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Preface 



Robotics applications, initially developed for industrial and manufacturing 
contexts, are now strongly present in several fields. Besides well-known space 
and high-technology applications, robotics for every day life and medical ser- 
vices is becoming more and more popular. As an example, robotic manipula- 
tors are particularly useful in surgery and radiation treatments, they could be 
employed for civil demining, for helping disabled people, and ultimately for 
domestic tasks, entertainment and education. Such a kind of robotic appli- 
cations require the integration of many different skills. Autonomous vehicles 
and mobile robots in general must be integrated with articulated manipula- 
tors. Many robotic technologies (sensors, actuators and computing systems) 
must be properly used with specific technologies (localisation, planning and 
control technologies) . The task of designing robots for these applications is a 
hard challenge: a specihc competence in each area is demanded, in the effort 
of a truly integrated multidisciplinary design. 

This monograph stems out of the research project RAMSETE (Robotica 
Articolata e Mobile per i SErvizi e le TEcnologie — Articulated and Mobile 
Robots for SErvices and TEchnologies) , funded in 1999-2000 by the Ral- 
ian Ministry for University, Scientific and Technology Research (MURST), 
involving a significant portion of the Ralian robotics community; namely, 
the research groups at: University of Bologna, University of Genoa, Poly- 
technic of Milan, University of Naples, University of Perugia, University 
of Pisa, University of Rome “La Sapienza”, University of Rome “Tor Ver- 
gata”. Third University of Rome, and Polytechnic of Turin. At the web 
site http : //www-lar . dels .unibo . it/ramsete a complete description of the 
project is available. 

Early research activities in Italy in the field of robotics date back to the 
late fifties. At that time, simultaneously with other activities in the United 
States, an extremely versatile master/slave tele-manipulator was under de- 
velopment at CNEN laboratories: the MASCOT. Some of the new versions 
of such manipulator are still in operation at the Joint European Torus lab- 
oratory, United Kingdom, were fusion reactors are under study. Since that 
time, the Ralian scientific community has always played a major role in the 
robotics field, with a new considerable interest about the end of the seventies 
when, downstream of the applications in manufacturing, some components of 
the Ralian academy started giving contributions of relevant scientific interest 
in the field of articulated robotics for industrial applications in structured en- 
vironments. Their contributions to the development of modern robotics were 
recognised by the whole international community. Among many others, rel- 
evant results have been given in the areas of dynamic modelling and control 
of articulated robots, adaptive and learning control, modelling and control 
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of elastic robots, asymptotic state observers, design and realisation of end 
effectors. 

The main objective of this monograph is to provide robotic researchers 
and developers with: a) a reliable and authoritative source of information, b) 
meaningful and feasible developments, c) signihcance and impact of new prob- 
lems in the field. The monograph emphasises all the implementation aspects 
related to major robot subsystems, i.e. both technology structures and spe- 
cific methodologies. The study of robotic subsystems aimed at the integration 
in a single complex system, with autonomous motion capabilities, represents 
the current frontier of scientihc research in the field of robotics. In particular, 
the most relevant open problems deal with the integration of functions and 
autonomous decision capabilities of articulated robots, such as control of co- 
operating arms, their interaction with the environment (contact and impact 
phenomena), as well as the integration of functions and autonomous decision 
capabilities of mobile robots, such as those related to localisation, planning 
of tasks and motion, and motion control. In addition, in all these situations 
the following factors attain a crucial importance: hardware/software archi- 
tectures for computation and control, availability of advanced actuators and 
sensory systems. 

The technical issues arising in the study of the above problems are exam- 
ined in the perspective of the required skills. In particular, there are some 
technological and methodological skills, that are necessary for the effective 
realisation of robots for field and service applications. In this monograph, 
these skills have been arranged into six methodological areas, three of them 
focused on articulated robots and the others on mobile/ autonomous robots, 
and four technological areas. The areas are listed below and the research 
group leading the activities during the project within each area is indicated. 

— Technological areas 

Hardware/software architectures (Univ. Genoa) 

Joint actuators (Polytech. Milan) 

End effectors (Univ. Pisa) 

Sensors (Univ. Bologna) 

— Methodological areas 

Articulated robots 
Flexible link control (Univ. Perugia) 

Interaction control (Univ. Naples) 

Impact modelling and control (Polytech. Turin) 

Mobile and autonomous robots 
Vehicle control (Univ. Rome “La Sapienza”) 

Planning (Third Univ. Rome) 

Localisation (Univ. Rome “Tor Vergata”) 
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The book is intended for graduate students, researchers, scientists and 
scholars who wish to broaden and strengthen their knowledge in robotics. 

Besides thanking all the Authors for their valuable contributions to this 
monograph, we wish to extend our appreciation to all the participants to the 
RAMSETE project who have produced significant research results during 
the latest two years. Warmest thanks are for Nicholas Pinfield, Engineering 
Editor, and his assistants Hannah Ransley and Oliver Jackson of Springer- 
Verlag, London. A final word of thanks goes to Costanzo Manes for the 
pictorial illustration below. 
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Many of the aspects related with the definition and development of a func- 
tional and algorithmic architecture for multiarm systems are dealt with in 
this chapter. Emphasis is given to the role played by each layer and to their 
composing basic functional blocks. In particular it is shown how such ba- 
sic blocks can be effectively used to the aim of incrementally constructing 
complex cooperative tasks, via suitable and simple interconnecting rules. An 
example of Hw/Sw architecture supporting the entire functional and algo- 
rithmic architecture is also provided. 



1 Introduction 

The problem of controlling and coordinating the actions of multiarm systems 
capable of performing complex cooperative operations, such as for instance 
coordinated object handling and transportation, coordinated assembly, etc., 
may result in a formidable task if not approached in a structured and well or- 
dered way. To this end, any well structured approach to the problem should 
actually involve three different aspects, which are however very correlated 
among them; namely: a) the functional and algorithmic architecture of the 
overall coordination and control system, b) the possibly distributed Hw/Sw 
architecture supporting the functional and algorithmic one, c) the efficiency 
and quality of the tools and environments used for developing the correspond- 
ing real time Sw. 

Moreover, as clearly stated in [15], the “demand for an organisation or 
structure that relies on well defined concepts to enable the effective realisation 
of complex system” is clearly recognised for each one the cited items. As a 
matter of fact, research in these interrelated areas of interests for robotics 
has progressively increased, in parallel with the theoretical and technical 
progresses achieved in robot control, real-time operating systems and software 
engineering as well and, certainly, it has also been driven by the increasing 
progresses in computer technology. 

As a matter of fact, all a set of very challenging experiments, up to now 
performed (like for instance the Rotex arm experiment in space, the Sojourner 
sytem exploring Mars, or also the P2 and now Asimo walking humanoids, just 
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to mention some among the most impressive ones) have clearly brought into 
evidence the effectiveness of the results and performance that can be obtained 
whenever complexity is approached in a well structured and organised way. 

A very good survey and collection of selected papers, covering each one 
of the mentioned areas, is represented by [1], where the above items are pre- 
sented from both the conceptual and implementative point of view, and where 
a set of general robotic examples are also given, together with indication rel- 
evant to the real-time Os and robotic oriented Sw developing tools available 
at that time (see for instance [5], [6], [19] as representative papers dealing 
with the above three items respectively). 




Figure 1.1. Underwater test bed 



Naturally enough, as it concerns this last item, further improvements and 
also new products actually emerged during the successive years, even without 
introducing substantial changes in overall approach philosophy accessed at 
that time. Among the three previously mentioned items, and possibly due to 
the peculiarities exhibited by the robotic field, a key role is however played 
by the first one (i.e. functional and algorithmic architectures) which sub- 
stantially drove the robotic oriented part of most of the proposed developing 
environment. 

Within this chapter, many of the aspects related with the development of 
a functional and algorithmic architecture for multiarm systems is addressed 
in some details. Special emphasis is given to the structure of the various 
layers composing the architecture and, more in particular, to the different 
building blocks and the mechanisms for their composition, within the execu- 
tion of complex cooperative tasks. To this end, and without loss of generality, 
reference will be made to the dual arm working cell reported in Figure 1.1, 
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composed by two 7-DOF electrically actuated arms, and employed for un- 
derwater applications [14], [13], [10], [11]. Moreover, just as an illustrative 
example, some little details concerning the adopted distributed Hw/Sw ar- 
chitecture implementing the overall functional one will also be provided, to- 
gether with some general indications about the employed developing tools 
and environments. 



2 The Functional Architecture 



The overall functional architecture adopted for the command and control 
of multiarm systems corresponds to the one reported in Figure 2.1, here 
specialised for the considered case of two arms. 
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Figure 2.1. Functional architecture for the dual arm workcell 
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As many others robotic functional architectures, it also is organised as 
a three/four- level hierarchical structure, where each level reflects a layer of 
abstraction of the solution of the global command and control problem. As a 
consequence, this results in an efficient yet flexible framework for the organ- 
isation of the interaction of relevant hardware/software components. To this 
end the description which follows will also clarify the rationale underlying 
the specihcally proposed multilayered structure. 

By analysing the hierarchy starting from the bottom, we first hnd the so- 
called “Very Low Level Control” (VLLC) layer, in correspondence of which 
joint velocity control is independently performed for each one of the arms, 
being the relevant reference signals provided by the next upperlying level. 
Typically, in practical applications (as it actually is, for the one referenced 
here) the VLLC is directly provided by the power drives of the joint motors, 
each one embedding a simple “high gain” PI velocity regulator. In alternative, 
more sophisticated controllers, like PI regulators plus exact or approximate, 
or even adaptive, nonlinearity compensation methods, could also be employed 
at this level. In both cases, however, the net result associated with the control 
laws implemented at this level is that of reducing the behaviour of each one of 
the subsystems “VLLC + Arm” similar to that of a bank of pure integrators, 
which directly transform (via time integration) the reference joint velocities 
into their corresponding joint positions. 

By proceeding upward, the next layer of the hierarchy is represented by 
the so-called “Low level Control” (LLC) accomplishing the function of in- 
dependently controlling the Cartesian position and orientation (or even im- 
posing the angular and linear velocities) of the tool frame of each one of the 
manipulators. More precisely, inside each one of the blocks composing such 
layer, the Cartesian position and attitude of the tool frame of the associated 
manipulator are real-time evaluated on the basis of joint position measure- 
ments and knowledge of the corresponding robot geometry. The output of 
such evaluation represents the Cartesian feedback information which is com- 
pared, inside each LLC block, with the corresponding Cartesian command 
provided by the next upperlying level. The further processing performed in- 
side each one of the LLC blocks finally produces, as outputs, the reference 
joint velocity signals to be input to the corresponding VLLC block. 

Still proceeding upward along the hierarchy we then find the so-called 
“Medium Level Control” (MLC) layer, whose function is that of suitably co- 
ordinating the motions of the manipulators tool frames in correspondence of 
any activated task. This is done via the automatic generation of the most 
suitable Cartesian reference positions and orientations to be tracked by the 
robots tool frames. Obviously enough, inputs to the MLC layer are the cur- 
rent positions and attitudes of both tool frames (provided as feedback by 
the underlying LLC layer) plus the commands specifying the tasks to be 
activated. 
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At the next level we further find the so-called “High Level Control” (HLC) 
layer, implementing the methods for the automatic composition (sequential 
and/or parallel) of tasks into more complex ones (macro-tasks) plus the rele- 
vant scheduling techniques. The outputs from HLC are consequently the task 
commands for the underlying MLC block. Inputs to HLC are instead those 
provided as feedback by MLC, which inform about the current state of execu- 
tion of the activated tasks, plus the request commands for new macro-tasks 
to be successively performed. 

Finally, the hierarchy ends up with the upmost level, simply represented 
by the “Human Computer Interface” (HCI), hosting the whole interactive Sw 
needed to the operator for efficiently commanding and monitoring the entire 
system. 

As it should be now apparent, the proposed functional architecture clearly 
results in a hierarchy organised in such a way as to maintain its lowest layers 
VLLC and LLC (the first operating at joint level and the second at the 
“interface” between joint and Cartesian space) within an algorithmic and 
processing structure which is always independent, and consequently invariant, 
with respect to the variety of tasks to be executed; being the variability with 
the tasks of the associated control schemes strictly confined within the upper 
MLC and HLC layers of the architecture. 

As it will be better clarified in later sections, such variability of the con- 
trol schemes, at the MLC layer, can however be efficiently handled by simply 
enabling, in correspondence of each task to be activated, the right intecon- 
nections among the elements of a restricted set of pre-defined, invariant, ele- 
mentary building blocks. Obviously enough, the function of structuring such 
interconnections among the basic building blocks is instead assigned to the 
HCL layer. Moreover note how such upper layers MLC and HLC, since to- 
tally operating at a Cartesian level, actually result to be largely independent 
from the specific geometry and kinematic of the underlying mechanical struc- 
ture (here “only occasionally” represented by a couple of 7-DOF manipulator 
arms) . 

Before concluding the present section, finally observe that, when needed, 
even the so-called “Motion and Interaction Control” (i.e. the simultaneous 
control of both motion and reaction forces/torques arising during the inter- 
action with the environment and/or manipulated objects) can also be per- 
formed at the MLC layer of the architecture (thus extending its operative 
space from geometric-Cartesian only, to geometric-Cartesian -|- force/torque- 
Cartesian) provided that suitable force/torque sensors feedback information 
is made available at this level. In this case, however, since the underlying 
LLC blocks can only receive position/orientation or linear /angular velocity 
commands, the MLC must consequently perform the force/torque interaction 
control via the generation of “equivalent” Cartesian position and/or velocity 
reference signals, to be distributed to the underlying LLC blocks. This will be 
better illustrated within a specifically dedicated later section of this chapter. 




6 



G. Casalino, D. Angeletti, T. Bozzo, G. Cannata 



From now on, the present chapter will proceed with an illustrative exam- 
ple of a possible Hw/Sw architecture supporting the above presented func- 
tional one, while the remaining sections will be dedicated to a more detailed 
description of each layer and related, composing, functional elements. 



3 An Example of Supporting Hw and Sw Architecture 

As it has been already mentioned, the definition of a functional architecture 
also provides a framework for the organisation of the interaction of relevant 
Hw/Sw components. In this section, an illustrative example of this fact is 
briefly presented with respect to the referenced dual arm workcell of Fig- 
ure 1.1. 



Host PC 
HCI 



LAN with TCP/IP 



MVME167 



VME BUS 



MVME162 



EO 



MVME162 



EO 



DSP 

JR3 



DSP 

JR3 



A 



Figure 3.1. Hardware architecture description 



The adopted Hw architecture corresponds to the one indicate in Fig- 
ure 3.1, where a standard distributed commercial architecture, based on VME 
bus and Motorola 68040 processing units, have been used. In this framework 
the two MVME 162 boards (one for each arm) host the LLC layer, while the 
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single board MVME 167 is used for hosting both the MLC and HLC layer. 
(The VLLC layer is instead embedded within the power drives, not shown in 
the figure, directly acting on the motor joints). The HCI is instead hosted by 
a PC interacting with the MVME 167 via LAN connection. 

The two MVME 162 boards directly acquire the encoder signals provided 
by the power drives, via local encoder interfaces installed on the boards, and 
provide analog voltage commands to the power drives via D/A converters, 
they also directly installed on the same boards. 

All the remaining data exchange among the three boards occur on the 
VME bus, on the basis of sharing memory communication techniques. The 
same kind of data exchange is also established between the MVME 167 board 
and the two DSP based force/torque sensor acquisition units, each one asso- 
ciated with the relevant wrist force/torque sensor of the corresponding arm. 

Communications between the HCI hosting PC and the entire system are 
instead established by the LAN connection with the MVME 167 board, via 
the use of TCP/IP protocols (sockets). Computing board MVME 167 also 
acts as “master board”; thus meaning that all the synchronisation aspects 
concerning data exchanges and communications are handled, within the ar- 
chitecture, in a centralised way. 

The overall Hw architecture is driven under the supervision and control 
of the real-time distributed VxWorks (of Windriver) operating systems. A 
common and synchronised sampling time of 5 ms could be chosen for all 
the control processes running on the different computing boards; this apart 
the processes regarding the regularised pseudo- inversion of some Jacobian 
matrices, performed via SVD algorithms, whose sampling time had to be 
hxed to 30 ms due to their greater computational burden (thus meaning 
that the upgrading of a Jacobian matrix always occurs every five sampling 
intervals of 5 ms, which however turned out to be acceptable in terms of 
obtained performances) . 

Naturally enough, even other types of computing boards, buses, and real- 
time OS (like for instance Qnx or RT-Linux, just to mention some of the 
more mature ones by now) could also be used, without however changing the 
underlying, functionally driven, Hw/Sw design philosophy. 

At this point, in order to conclude the example, a brief mention of the 
used design and developing tools could also be useful. More specifically, the 
development of the real time supporting Sw architecture has been done on 
the basis of the real-time Sw developing and debugging environment Tor- 
nado (of Windriver), the development of the C-code corresponding to the 
various functional blocks (see forthcomimg sections) has been done via the 
Matlab/Simulink/RT-Workshop (of The Mathworks) environment (simula- 
tion, automatic C-coding and downloading), while the C-coding of specific 
robotic functional blocks, such as transformation and Jacobian matrices, etc., 
has been obtained automatically by exploiting the facilities offered by the so- 
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called RDS (Robotic Developer Studio) [11], [17]: a proprietary environment 
devoted to symbolic modelling and simulation of complex robotic systems. 



4 The Low Level Control Layer 

Consider Figure 4.1, where the kinematic structure of one of the 7-DOF 
arms of the workcell is schematically represented. In this figure, Tg is the 
transformation matrix of the end-effector frame < e > w.r.t. base frame 

< o >, T is the (constant) transformation matrix of the tool eenter frame 

< t > w.r.t. < e >, and T* the (generally time varying) transformation 
matrix of the reference frame < g > (w.r.t. < o >) to be tracked by tool frame 
<t>. Moreover, vectors r and p (both projected on the base frame < o >) 
represent the distanee and the misalignment (equivalent rotation vector) of 
reference frame < g > w.r.t. < t >. By collecting together the two error 
vectors, the six dimensional global error vector e is defined as e = [r^,p^]^. 




Figure 4.1. Schematic representation of a single arm 



The control scheme for a single arm is schematically depicted in Figure 4.2, 
where the block “Robot -I- VLLC” represents the physical arm equipped with 
its seven joint drivers, each one implementing a closed loop velocity control 
at the corresponding joint. Thus such block can be seen as a compact one 
receiving reference joint velocities as input, while giving the eorresponding 
joint positions as their time integral output. 

The remaining part of the scheme, termed as the “Low Level Control” 
(LLC) loop, is instead composed by the interconnection of the following 
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Figure 4.2. Single arm control scheme 



blocks. The processing block P, used for real time evaluating the global error 
e via the solution of the well known “versor lemma” equations [12], [4], for 
its rotational part p, and via the difference between the first three elements 
of the last columns of T* and Tt, respectively, for its linear part r. The gain 
matrix gl {g > 0), used for translating the global error e into the generalised 

Cartesian velocity vector X — [o)^ (also projected on < o >) to be 

assigned to the tool frame Tf in order to have the convergence of e toward 
zero. Then we have the additional Cartesian velocity input X* , only used 
for coordination purposes (as described in Section 4), and moreover block S, 

translating X into the related Cartesian velocity X for the end-effector frame 
< e >, via the use of the well known rigid body velocity relationships 

j Ul = Ul (A^ \ 

u = D -I- [sAjcD ' ' ' 

where s is the vector distance (projected on < o >) of frame < t > with 
respect to < e >; i.e. vector 

s = Ret (4.2) 

being Re the rotation matrix part of the end-effector frame transformation 
matrix Te, and t the same (constant) distance vector projected on the end- 
effector frame < e > (i.e. the hrst three elements of the last column of the 
constant transformation matrix T). 

Finally, within the same scheme, block H is used for implementing the 
simple transformation relationship 

Tt = TeT. (4.3) 

The end-effector Cartesian velocity reference signal X, generated as indicated 
above, is in turn transformed, via the functional block Q, into a correspond- 
ing set of joint velocity reference inputs, globally represented by the seven 
dimensional vector q. 
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In Figure 4.1, the sub-scheme enclosed in the inner dotted box constitutes 
what is generally termed as the “Interface between the Cartesian and the joint 
spaces” of the overall scheme. As a matter of fact, within such interface part 
block Q is also required to cope with aspects that generally go a little beyond 
the simple pseudo- inversion of the Jacobian matrix. 

Just as an illustrative example, in the following we briefly recall the nature 
of such aspects by showing how they have been handled within the specific 
case of the considered workcell. 




Figure 4.3. Singular configuration 



To this aim consider again the redundant arm structure as depicted Fig- 
ure 4.3, showing the most important singular configuration exhibited by the 
arm. As it is well known, in the vicinity of such configuration, joint veloci- 
ties produced via simple pseudo- inversion of the Jacobian matrix would tend 
to infinite values; thus inducing (at least temporarily) instabilities and not 
acceptable vibrations within the whole structure. Thus the need for regulari- 
sation naturally arises, together with that of maintaining the arm sufficiently 
“far” from such configuration though. Then, in order to satisfy the above re- 
quirements, the following algorithm has been finally adopted for generating 
q from the inside of the interface block Q 

q = - h4<l4) + q (4.4) 



being 



q=[0 0 0 94 0 0 0]^ 

94 = -fc(94 - 94). 



(4.5) 
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The rationale underlying the above expression can be simply explained as 
follows. First of all note that term q corresponds to an elastic feedback tending 
to maintain the fourth joint far from its zero value. The relevant elastic gain 
has been chosen in such a way as to substantially act only in the vicinity 
of the zero position, thus resulting in a finite support, bell shaped, positive 
radial basis function of centered on zero. Due to the presence of q, the 
first term in (4.4) is consequently used for both compensating its Cartesian 
effects h 4 q (/14 is the fourth column of the end-effector Jacobian matrix J) 
while also guaranteeing the tracking of the required Cartesian velocity X. 
This obviously corresponds to perform a projection of q on the null space 
of J, which can however be done by minimally depressing the elastic action 
given by q^, via the use of the following Jacobian weighting matrix W 

VF=diag[l, 1, 1, tc, 1, 1, 1] w=l + k. (4.6) 

Finally note in (4.4) how the term A is also added for regularisation pur- 
poses, taking also the form of a finite support, bell shaped, positive radial 
basis function (centered on zero) of the determinant of the matrix 
(or, even better, of the ratio between its minimum and maximum singular 
value) [18]. With the adoption of the above technique, it can be shown that 
singularity can always be avoided provided that X is guaranteed to always lie 
within specific norm bounds, which can however be enlarged for increasing 
values of k [ 12 ]. 

The structure of the overall scheme of Figure 4.1 has been proposed by 
some of the authors since 1995 [2]. Its nice stability and robustness properties, 
with respect to possible uncertainties of both dynamic and geometric nature, 
has also been proved by the authors within different reports and papers. 
Theoretical details can be found for instance in the works [2], [7], [12], [4], 
[3]. 



5 The Medium Level Control Layer 

As discussed in Section 2, the major function to be accomplished by the MLC 
layer is that of guaranteeing motion coordination during the execution of a set 
of tasks generally involving both the arms; being those requiring the motion 
of a single arm only considered as a very special case of the wider class of 
cooperative ones. Nevertheless, since the capability of efficiently structuring 
the motion of a single arm, actually constitutes the fundamental framework 
for similarly structuring more complex cooperative tasks, the present section 
will consequently start with a discussion about single arm motion tasks, while 
a treatment of the more general cooperative ones will follow immediately 
after. 
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5.1 The Teleoperated/ Automatic Independent Motion Control of 
the Arms 

A “Teleoperated Motion Control Mode” of a single arm is intended to be 
established when the reference frame to be tracked by each arm is made to 
move around space by integrating velocity data (angnlar and linear) acquired 
from HCI, via the use of suitable “space mouse” devices interacting with the 
HCI itself. However, as it will be better explained shortly, a simple integration 
of the velocity data coming from the space mouse devices may actually not 
be effective for driving each one of the arms. 

As a matter of fact, the more complex functional diagram depicted in 
Figure 5.1 has revealed to be the most suitable one for teleoperation purposes 
[12]. In order to explain the rationale underlying the use of the proposed 
scheme, let us first assume for a while that blocks named gl and gl simply 
correspond to the identity and the null one, respectively (i.e. g = 1 and 
g — 0); then in this case the diagram directly reduces to the sole integration, 
performed by the block named Int, of the acquired absolute velocity data u>g 
and Vg assigned to the goal reference frame < g >■ 




Figure 5.1. Functional diagram for teleoperation purposes 



Obviously enough, provided that the absolute velocities data tOg and Vg 
are interpreted as projected on the absolute frame < o >, block Int results in 
performing the time integration of the following differential equations (with 
the first of the two efficiently integrated via the use of the well known “Ro- 
driguez formula” [16]) 



f i?* = [uJgA]R* 
1 P* = Vg. 



(5.1) 



In the above equations the starting conditions are set to be equal to the initial 
posture assumed by the tool frame < t > of the arm. The resulting absolute 
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reference outputs R* and p* (rotation matrix and position oi < g > w.r.t. 
< o > projected on < o >) are then assembled together in order to give rise 
to the corresponding transformation matrix T*. It should be however noted 
that in this case, provided that the sole block Int is actually used, there might 
be the risk of having (at least temporarily) the tracking error e attaining very 
high values, before eventually converging to zero. 

This might for instance happen in case of a “fast” virtual motion of the 
space mouse or, more commonly, when the target frame < g > is pulled 
too far outside the robot workspace. In particular, since in this latter case 
the robot is progressively pushed toward its singularity, while trying to re- 
duce errors that instead (due to mechanical constraints) could even attain 
unbounded values, very high values for X could consequently be induced 
within the scheme of Figure 4.2. Then, just due to this, it turns out that 
the assumed norm bounds of X (i.e. those allowing the effective avoidance of 
singularity, as mentioned in Section 4) generally risk to be seriously exceeded. 

As a matter of fact, since this drawback could be avoided by simply forcing 
the tracking error e to remain confined within a specified superior norm 
bound, this is consequently done via the use of the block gl, having the 
gain g taking on the form of a finite support, bell shaped, positive radial 
basis function centered on zero, of the tracking error norm. As it can be 
easily seen, since the velocities of the target frame are now progressively 
reduced toward zero in case the error e approaches the specified norm limit, 
it consequently occurs that the error e itself cannot ever overpass such norm 
bound (obviously provided that it always starts from lower norm values). 

Nevertheless, with the sole use of the above described “norm error limiter” 
block it is clear that, in case of attainment of the specified norm error bound 
in correspondence of points located outside of the robot workspace (where 
the error itself cannot be anymore reduced via robot motion, and where 
the velocity gain g however attains the zero value) unfortunately there will 
not be the possibility of moving again the goal frame < g >: this due to 
the fact that, in the impossibility of reducing the error, its velocity will be 
consequently maintained at a null value by the gl block itself. Then, in order 
to avoid such additional drawback, a moderate, tracking error based, constant 
velocity feedback has been added via the constant gain block termed as gl. 
As a matter of fact, since such block always exerts a moderate recalling action 
on < g > itself, directed toward the tool frame < t > of the arm, it always 
prevents the error e to exactly reach its upper norm bound; thus always 
avoiding the total zeroing of the gain in the gl block. 

Obviously enough, when the arm is instead inside its workspace, the mo- 
tion capabilities of the arm itself (i.e. the capability guaranteed by the LLC 
gains of maintaining e inside its lower limits closed to zero, provided e itself 
starts from the inside of such limits) simply result in a “masking” of the 
(moderate) feedback action produced by the gl block. The final assessment 
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of the functional scheme reported in Figure 5.1 corresponds to what has been 
termed as the “MLC Teleoperated single arm” . 




Figure 5.2. Frame tracking arrangement for the automatic independent motion 



As opposed to the above described teleoperated mode, an “Automatic 
Independent Motion Control Mode” for a single arm is intended to be es- 
tablished when the end effector of one of the arms is required to reach any 
assigned position/orientation in space, starting from any initial posture. 

Within such kind of control (reentering within the more general category 
of the so-called “point-to-point” control modes) the position/orientation tra- 
jectory followed while reaching the final posture is automatically established 
by the implemented corresponding control loop. Then, it is just in this sense 
that such kind of control mode differs from the already considered teleoper- 
ated one, where the trajectory is implicitly assigned on line by the operator 
interacting with the system (via a reference velocity profile, generated by a 
space mouse device). 

In order to implement such kind of automatic control mode it should be 
however clear that, at least in principle, the sole LLC fundamental scheme 
of (Figure 4.2) would actually suffice, provided that it could be somehow 
assured that the induced internal Cartesian velocities remain norm bounded, 
in order to allow singularity avoidance (see Section 4). 

As a matter of fact, similarly to the teleoperated case, since a way for 
guaranteeing such possibility is still that of maintaining the global error e 
within a suitably assigned maximum norm bound, the conceptual scheme of 
Figure 5.2 can consequently be adopted. 

In such scheme, frame < s > is an auxiliary one that, starting from a 
posture coinciding with the tool frame < t >, is commanded to reach the 
goal frame < g > via the use of a Cartesian control loop whose feedback gain 
takes on the same form of a finite support, bell shaped, positive radial basis 
function centered on zero, of the norm of the error e between frame < s > 
itself and < t >. 
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MVME 167 




Figure 5.3. Medium Level Control for automatic independent motion 



Meanwhile frame < f > is commanded to follow < s > (and a moderate 
constant feedback action of the form gl still exists, weakly recalling < s > 
toward < t >). As it can be easily realised, this guarantees the boundedness 
of the tracking error e (of < s > w.r.t. < t >), and also its convergence 
to zero together with the other tracking error e' (of < g > w.r.t. < s >) 
provided < g > is inside the arm workspace. Otherwise {< g > outside the 
arm workspace) both the errors e, e' will only remain bounded (in particular 
with the norm of e inside its upper limit) eventually reaching the equilibrium 
condition 



9{\\eVy - ge = 0 (5.2) 

only satisfied by a corresponding vector e whose norm is necessarily strictly 
lower than its assigned upper bound. 

The functional scheme corresponding to the above conceptual one is re- 
ported in Figure 5.3. 

As it can be seen from the figure, the part of control concerning tracking 
of < s > by part of < t > is obviously let to the LLC of the arm. Only the 
remaining part of the scheme is implemented as MLC. 

5.2 The Teleoperated/ Automatic Coordinated Motion Control of 
the Arms 

Consider the situation depicted in Figure 5.4 showing the (assumed steady) 
condition when a lightweight rigid object has been firmly grasped by both the 
grippers, after a preliminary phase of object approach and consequent grasp- 
ing, performed via independent control mode. Then the same figure clearly 
evidences the need for being able to move both the end effectors without 
varying their mutual position and orientation; thus letting the grasped ob- 
ject consequently move properly. To this end also assume that, prior to any 





Figure 5.4. Teleoperated dual arm workcell handling an object 




Figure 5.5. Frame tracking arrangement for teleoperated coordinated motion 
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motion, the existing transformation matrix T between the two end effectors 
is preliminary and automatically evaluated (on the basis of the known robot 
geometry information) via the simple expression 

T = T,,T,^. (5.3) 

Then a tool center frame < tb >, having the constraint matrix T as trans- 
formation matrix w.r.t. < Cb >, is assigned to arm (b) while the tool center 
frame < ta > oi arm (a) is let to coincide with its end-effector frame < Ca >■ 

The teleoperated coordinated motion of the arms can be performed on 
the basis of a “parallel task composition” , to be implemented in the following 
way. 1) The tool frame < tb > is required to remain coinciding with the tool 
frame < ta >.2) The tool frame < ta > is required remain coinciding with the 
tool frame < tb >. 3) Meanwhile, the tool frame < ta > is required to closely 
follow the moving goal frame < g > (supposed to be initially coinciding 
with < ta >). Such situation, actually indicated in Figure 5.4, conceptually 
corresponds to what is reported in Figure 5.5, where the dotted arrow is used 
for indicating the “attractive” action contemporaneously performed on < ^ > 
toward < ta >, hy the (moderate) feedback gain gl, still accomplishing the 
same role as that described in Section 3. 

As it is apparent from Figure 5.5 (and in accordance to what has been 
above established) while frame < tb > is required to track a single frame 
(actually frame < ta >) and frame < ta > is instead required to contempo- 
raneously follow two different target frames (namely, frame < tb > and the 
moving one < g >). The same obviously holds also for frame < g > itself, 
which is required to move around space while simultaneously tracking, due 
to the presence of the previously mentioned moderate feedback action, also 
frame < ta >■ To this regard, it must be noted that such seemingly con- 
trasting aspects actually are only of apparent nature: this simple due to the 
fact that the above established global task actually admits a unique solu- 
tion, contemporaneously fulfilling to all the composing subtasks (i.e. the one 
corresponding to have, eventually or just from the beginning, all the frames 
coinciding among them; see [8]). 

Seen from the MLC implementative point of view, the control scheme 
allowing the execution of the above specified overall task consequently results 
into that of Figure 5.6, corresponding to a suitable composition of the same 
control modules already defined within the independent control of the arms. 

Within this figure, the two blocks LLC (a) and (b) located at the right 
are the same control schemes, already used for the independent control of 
the two arms; with the difference that, now, each LLC layer block receives, 
as input reference frame to be tracked, the tool frame of the other. Moreover 
block LLC (a) also receives, as an additional input, the Cartesian velocity 
control signal X* generated by the external loop (the one enclosed in the 
inner dotted box) delegated to control the tracking of < g > by part of 
< ta >■ Further, the transformation matrix T* of the moving frame < g > 
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MVME167 




Figure 5.6. Gontrol scheme for teleoperated coordinated motion, corresponding 
to the conceptual scheme of Figure 5.5 



is generated via a mechanism (the leftmost block) which is the same of that 
described in Section 3. 

As it concerns this last block, a slight difference with the one used in 
Section 3 however exists. Such difference simply consists in the fact that now 
the block admits, other than the tracking error e between < g > and < ta > 
as inputs feedback (as before), also the “internal” one 6a between <ta> and 
< 4 >. In this case, while e is still singularly used as input to the constant 
feedback block gl, when joined with Ca it is instead employed for evaluating 
the quantity 

^=||ef + /i||eaf h>0 (5.4) 

which is now used as argument to the gain g of block gl (obviously still 
having the form of a finite support, bell shaped, positive radial basis func- 
tion centered on zero) whose finite support extends within the validity of a 
(suitably defined) threshold condition ^ < e. 

As a matter of fact, with the use of the above argument and related 
suitable choices for the weighting parameter h and threshold value e, it be- 
comes possible to move both the arms in a coordinated fashion while avoiding 
that both errors exceed the corresponding prespecified upper norm bounds; 
namely: ^ for Ca and e itself for e. 

The entire scheme globally enclosed within the dotted box in the left side 
of Figure 5.6 represents what is termed to be the “Teleoperated dual arm 
MLC”. 

Before proceeding further on, it might be of some interest to make some 
considerations concerning a possible variant to the previous scheme; i.e. the 
one corresponding to the “task composition” diagram of Figure 5.7 and the 
relevant control scheme of Figure 5.8. Within such variant, tool frame < h > 
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Figure 5.7. Another possibility of frame tracking arrangement for teleoperated 
coordinated motion 



is also required to follow the moving frame < g >, as it is for < ta > within 
the diagram of Figure 5.5. 



MVMEI67 




Figure 5.8. Control scheme for teleoperated coordinated motion, corresponding 
to the conceptual scheme of Figure 5.7 



As a result of such modification, the inherent priority previously existing 
between the tasks indicated in Figure 5.4 (resulting in a role of prevalent 
“master frame” substantially played by frame < ta >) is now totally lost 
in favour of a more balanced distribution of the tasks within the various 
involved all frames. Moreover, the entire set of frames now results to be 
“more bound” (by using a colloquial expression) than that of Figure 5.4, due 
to the redundant existence of additional feedback control actions. 

At this point the same idea underlying the Automatic Independent Mo- 
tion Control previously seen, can be easily extended to the case of Automatic 
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><8> <s> ► ^8> 





(a) 



(b) 



Figure 5.9. Frame tracking arrangement for the automatic mode 



Coordinated Motion Control via the use of one of the conceptnal schemes of 
Figure 5.9 (where the right figure is an improvement of the left). 
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Figure 5.10. Control scheme for automatic coordinated motion, corresponding to 
the conceptual scheme of Figure 5.9(a) 



To the above conceptual schemes there obviously correspond the func- 
tional ones of Figures 5.10, 5.11; which are in turn strictly analogous with 
the teleoperated ones of Figures 5.6, 5.8, but only differing for the internal 
presence of the functional block “MFC automatic single arm” in lieu of the 
teleoperated one. 

By following the same identical philosophy which has been adopted in 
Section 5, note how the block “MLC automatic single arm” now receives as 
input both the “external” tracking error e and the “internal” one ta- 
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fig-10 



Figure 5.11. Control scheme for automatic coordinated motion, corresponding to 
the conceptual scheme of Figure 5.9(b) 



5.3 Interaction Control during Coordinated Motion 

In the previous section, the problem of moving a grasped object in a co- 
ordinated fashion has been considered without paying any attention to the 
aspect of possibly controlling, during the motion, also the interaction forces 
and torques which could actually exists in correspondence of the grasping 
zones of the transported object. 

As a matter of fact, it could have been argued that the previously pro- 
posed control schemes substantially reflect the idea of actually “mimicking” 
the coordinated motion of a lightweight object; thus implying a transporta- 
tion that, in presence of a real lightweight body, consequently occurs with 
(substantially) null interacting forces and torques. 

In many applications this might actually be a requirement to be met (es- 
pecially when delicate, other than lightweight, bodies need to be transported) 
even if, certainly, it does not correspond to the most general one. In fact, there 
could be also cases where, for some reasons, the transported object needs to 
be maintained stretched, or compressed, or even torqued, during motion. 
For such additional cases, we shall now show how the previously proposed 
“mimicking” control schemes can be used as sort of “skeletons” , upon which 
more proper control schemes can be built, also allowing interaction control, 
whenever needed. 

To this end, let us restart by considering the initial grasping of a 
lightweight rigid object by part of both the manipulators, as depicted again 
in Figure 5.12. 

In this new figure, force F and torque M represent the possible interaction 
vectors acting on the end effector of robot (a) and projected on the corre- 
sponding frame < Ca >. In the same figure, the opposite of such force and 
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Figure 5.12. Forces and torques when the grippers are handling the object 



torque vectors instead constitute the interactions acting on the end effector 
of robot (b) whenever translated on its own tool frame < tb > coinciding (at 
least initially) with < Ca > as done in Section 5.2. Obviously enough, when 
existing, such interactions turn out to be one the opposite of the other, since 
they are referred to an assumed lightweight rigid body (then substantially 
with no mass and inertia). Moreover, since the rigid body is supposed to 
be firmly grasped, this implies that the geometric “internal errors” Ca = —Cb 
necessarily remain at their initial null values even during coordinated motion, 
just as a consequence of the assumed firm grasping constraints. 




Figure 5.13. Control scheme to realise the corrected signals to add in the reference 
velocities control loop 



Further, as pointed out at the beginning of this section, if the coordinated 
motion is performed as in Section 5.2, such motion will actually proceed with 
substantially null interactions. Now, provided that non-null (constant) refer- 
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ence vectors F* and M* are desired for the (otherwise almost null) interaction 
vectors F, M, the following very simple interaction feedback scheme can be 
used, in superposition with any one of the two of Section 5.2, as indicated. 

In this scheme (Figure 5.13) the global interaction reference vector 
/* = [F*^ M*^]^ is compared with the measured one / = [F^ 

— indifferently provided by one of the robot force/torque sensors; for in- 
stance the one mounted on robot (a) — and the corresponding interaction 
error vector F distributed (after multiplication by a constant gain matrix kl 
and successive projection on the absolute frame < 0 >) to both the LLC 
blocks in terms of a couple of equal and opposite additional Cartesian veloc- 
ity commands, non perturbing the overall coordinated motion. Notice that 
the proposed control loop is explicitly lacking of an integral action (generally 
appearing within schemes proposed in the literature) since such action is al- 
ready provided “internally” by the presence of the existing PI joint velocity 
regulators located at the VLLC layer. Then, the proposed simple loop also 
guarantees the regulation to zero of the associated interaction error vector 
F. Additional details of the resulting overall coordinated control scheme can 
be found in [9] . Extensions of the overall coordinated motion and interaction 
control schemes to more general cases of slightly deformable, or even fully 
deformable, lightweight objects, are currently under development. 



6 Comments on the Role of the High Level Control 
Layer 

The previous section has brought into evidence how different types of inde- 
pendent and cooperative tasks can be accomplished via the use of associated 
control schemes, each one of them resulting from the suitable interconnec- 
tion of a limited set of “primitive” or “basic” (in a sense) functional building 
blocks, which appear to be shared by all the task control scheme up to now 
considered. 

It is the authors’ opinion that such sort of “composition property” could 
actually be maintained (possibly by slightly enlarging the set of basic func- 
tional blocks) even in the presence of any possible grow in the number and 
type of tasks which could be asked to be performed by the multiarm cell Then, 
in this perspective we can consequently think of assigning to the HLC layer 
the function of activating the right MLC scheme upon request of execution of 
the corresponding task. Obviously enough, this can be done in many different 
ways, ranging from a straightforward one, just corresponding to the activa- 
tion of only one among a finite number of separate programs or schemes (one 
per task), to the more involved, requiring the automatic extraction, within 
an enlarged superset of admissible possibilities, of the right interconnections 
among the basic functional blocks (i.e. the right organisation among a set of 
basic routines) upon request of execution of the corresponding task. 
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In case of few tasks to be activated (as it is for instance for the number 
of tasks, still very limited, considered in this work) the first solution actually 
reveals to be the simplest to be implemented and even the best performing 
one (just for this reason it corresponds to the one currently adopted within 
the referenced experimental system of Figure 1.1). 

In the opposite case, the more complex involved tasks would reveal to 
be more advisable, since allowing much more flexibility in terms of modifi- 
cation, diversihcation, and/or addition of new tasks, without requiring any 
substantial coding effort, after the initial developing stage. Progress toward 
the realisation of an HLC layer of the second type should however proceed in 
parallel with the sought further progress in the construction of a structured 
framework for classes of cooperative tasks much wider than the one, still very 
simple, which has been considered in this work. 

Another set of problems which must be taken into account in the construc- 
tion of the HLC layer (in a form or in another) is represented by the possible 
“spike disturbances” which might be induced in the mechanical structure dur- 
ing the short duration of the switching time between different MLC schemes. 
As a matter of fact, in order to overcome such problems, particular atten- 
tion must be paid to two complimentary aspects; namely: a), the theoretic 
conditions (in terms of input, output, and error values, plus logic conditions) 
which allow smooth transitions from different tasks and b), the need of not 
allowing any “empty” sampling interval during transition. 

As a matter of fact, while the second aspect can only be handled by care- 
fully acting at the Os level, the first one is a problem of analysing in details the 
possibility of directly sequencing (in the mentioned terms) different couples 
of tasks. In particular, the foreseen need of defining “auxiliary tasks” allow- 
ing the sequential connection of tasks, that otherwise cannot be sequenced, 
should also be kept into account as a possibility for further developments. 
Up to date, with reference to the experimental system of Figure 1.1 and for 
the class of tasks considered here, such analysis has been performed, almost 
trivially, on an inspection basis only. 

From the above latter considerations a more precise characterisation of 
the role to be played by the HLC layer follows directly; namely that of an 
event based closed loop logic controller, managing the overall system. Just due 
to this, the cluster HLC-I-MLC naturally acquires characteristics which are 
proper of the so-called “Hybrid Control Systems” , thus allowing the correct 
positioning of the overall multiarm control and coordination problem within 
the framework of one of the most promising current research areas in the field 
of Automation. 



7 Conclusions 

Within this chapter, many of the aspects related with the development of 
a functional and algorithmic architecture for multiarm systems have been 
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addressed in some details. Special attention has been directed toward the 
definition of the roles to be played by the various layers in the perspective of 
reflecting, at each layer, a specific level of abstraction of the solution of the 
global command and control problem; thus also leading to an efficient yet 
flexible framework for the organisation of the interaction of relevant hard- 
ware/software components. 

Particular emphasis has been given to the various basic functional blocks 
characterising each layer of the architecture, whose composition possibilities 
have revealed to be very useful in the perspective of being able to incre- 
mentally construct (via suitable composition rules) wider classes of complex 
cooperative tasks. Some comments concerning the relevant real-time Hw/Sw 
design aspects have also been given, together with those concerning further 
research directions to be undertaken. 
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Two major sources of load oscillations in servo systems are dealt with in 
this contribution, namely pulsating torque disturbances and torsional flexi- 
bility. After reviewing the state of the art in torque ripple compensation, a 
compact model of torque disturbances is presented and algorithms are given 
for the identification of model parameters. Thanks to its compactness, the 
model allows the implementation of a straightforward and effective distur- 
bance compensation technique. Torsional flexibility is modelled by a classical 
two-mass model with spring and damper. A short review on the control of 
elastic joints is given. Then a careful analysis of the properties of the two- 
mass model is performed, leading to the design of a P/PI controller where 
suppression of load oscillations rather than fast setpoint tracking of the mo- 
tor position is pursued. Further improvements of the load behaviour can be 
achieved by a notch filter placed outside the velocity loop. Finally, the most 
complete solution available with only motor position measurements, an LQG 
controller with feedforward actions for load setpoint tracking, is investigated. 
Experimental and simulation results are given to assess the effectiveness of 
the proposed approach. 

1 Introduction 

Positioning servomechanisms are used in a large number of applications in 
robotics, machine tools, packaging, printing and textile machines, entertain- 
ment products, computer peripherals and devices, space and defense point- 
ing, motion systems and other applications. The large majority of servos use 
permanent magnets motors connected to the load by a transmission chain 
(or gearbox), and a single position sensor, either an encoder or a resolver, 
mounted on the motor shaft. This is by far the most common solution adopted 
in articulated robotic manipulators. 

By feeding back the motor position only, it is relatively easy to obtain sat- 
isfactory control of the motor position and velocity, using either P /PI or PID 
control, or more advanced compensation techniques [15], [24], [30], [29], [32]. 
However, this does not guarantee a satisfactory control of the load position 
and velocity, for demanding motion control applications, in particular during 
slow motion. For instance, oscillations of the tip of a manipulator arm or of 
the tool of a milling machine may arise. A large variety of sources of oscilla- 
tion can be identified: motor [5], [13], [20] and gearbox [10] pulsating torque 
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disturbances, torsional elasticity of the transmission chain, sensor noise [11], 
friction [2], backlash, and others. Among them, special attention should be 
paid to the transmission elasticity, which generally causes the lowest ( “first” ) 
resonance frequency of the positioning system, and to the motor torque dis- 
turbances (ripple), that mostly excite the resonance at low velocities [7]. 

This paper deals first with the compensation of motor torque distur- 
bances, and then with the design of the “standard” P/PI controller, car- 
ried out emphasising a well damped behaviour of the load rather than a fast 
setpoint tracking of the motor position. 

As far as the torque ripple suppression is concerned, several approaches 
have been proposed [4], [12], [13], [16]: some of them will be reviewed in 
Section 2.2. In the present contribution, the problem is addressed starting 
from the formulation of a compact model of the torque pulsations in sinusoidal 
permanent magnet AC brushless motors (PMAC BLM) [5] . The model defines 
the torque delivered by the motor as a scalar function of the rotor position 
and avoids addressing the behaviour of each phase for the identification of 
its parameters. 

Methods and algorithms are then proposed for the identification of the 
disturbance model, starting from spectral identification methods. They re- 
quire a controlled motion at constant low velocity: information about the 
torque pulsations is extracted from the output of the closed loop controller 
and correlated to the angular position of the rotor. Then, in order to cope 
with the time varying nature of the fundamental harmonic of the disturbance, 
a methodology for online adaptation of the parameter estimates is proposed. 

A compensation technique is discussed as well. The idea is to modify on- 
line the current reference produced by the position controller as a function of 
the rotor position. As such, the algorithm can be performed directly in the 
position controllers, even though modifications of the lookup tables used to 
compute the brushless functions for the single motors would be feasible as 
well. Experimental results are given to show the effectiveness of the compen- 
sation technique. 

Some existing approaches to the control of elastic joints will be briefly 
reviewed in Section 3.2. As a matter of fact, load vibrations can be reduced 
by a proper design of the P/PI controller, based on a careful analysis of the 
properties of the two-mass model of the elastic system. As a consequence of 
the relative position of poles and zeros of the process transfer function, it 
is straightforward to show, using the root locus analysis, that the common 
practice of increasing the bandwidth of the velocity loop (high gain of the 
PI regulator) fosters the oscillation of the load. It is then suggested a way 
to choose the loop gain, trading off between higher values, for a fast velocity 
response, and lower values, for more damped oscillations of the load. Further 
improvement of the damping of the closed loop dominant poles can also be 
obtained with a proper tuning of a notch filter that, however, has to be placed 
outside the velocity loop. 
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Modern control theory suggests that the most complete solutions for the 
control of the system consists in feeding back the estimates of the whole state 
of the system, obtained through suitable observers or Kalman filters [1]. This 
solution has been adopted in the past [21] and is investigated here, emphasis- 
ing the choice of the closed loop poles, based on control effort and sensitivity 
considerations, and the design of the feedforward blocks for accurate tracking 
of the load position setpoint. 

Modelling, identification and compensation of pulsating torque distur- 
bances are dealt with in Section 2, where experimental results are also given. 
Section 3 discusses first the properties of the two-mass model, then deals with 
the P/PI design and finally with the notch filter design. Experimental and 
simulation results are given. Section 4 presents the design and experimental 
validation of the LQG plus feedforward controller. 



2 Modelling and Compensation of Torque Disturbances 

2.1 Torque Disturbance Generation 

Consider the functional scheme of a sinusoidal PMAC machine, represented 
in Figure 2.1. 





Figure 2.1. Functional scheme of a brushless motor 



If a reference torque f should be delivered by the motor, typically as 
required by a position controller, the current reference I has to be given the 
value I = f/Kt, where Kt is the torque constant. This scalar setpoint is then 
modulated through three sinusoidal functions of the electrical angle a = PQm, 
p being the number of pole pairs and qm being the motor angle, that are offset 
by 27 t/ 3 one from each other. The three resulting signals become the current 
setpoints for the three phases. High bandwidth current controllers make the 
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currents track their setpoints in each phase. If the current reference in each 
phase is given the same dependence on the electrical angle characterising 
the back EMF (ideally sinusoidal or trapezoidal), a torque t is produced, 
approximately equal (in a band of frequencies limited by the current loops) 
to the desired torque r, and thus proportional to the scalar current reference 

7 . 

The behavionr of a real motor differs however from the simple one here 
described. Several constructive imperfections of the motor and the servodrive 
sum up to form a pulsating disturbance on the torque. Cogging torque and 
pulsating torque due to offsets in the current drives are examples of pulsating 
torque disturbances not related to the current reference. Cogging is mainly 
due to the presence of slots in the stator, where the rotor generated magnetic 
flux seeks a minimum reluctance path. It is present even when the drive is 
not connected to the power supply and depends in a periodic way on the 
mechanical angle. When the drive is connected to the power supply and the 
current reference is zero, a current offset may be present in one or both 
the closed loop controlled phases, and thus on the third one. These currents 
generate an offset torque periodic with period 27 t with respect to the electrical 
angle a. 

Pulsating torques related to the current reference are dne to imperfections 
in the construction of the motor and the drive, implying that both the back 
EMF profiles and the phase currents may be affected by undesired higher or- 
der harmonics, while obviously maintaining the periodicity of 27 t with respect 
to the electrical angle. 

2.2 Existing Solutions for Torque Minimisation 

A complete review of the methods used to minimise torque disturbances is 
reported in [20]. The techniques are classified into two main categories: motor- 
based, where the fundamental electromagnetic sources of the disturbance are 
addressed, and the design of the motor is adjusted in order to minimise them, 
and control-based, where active control schemes modify the excitation, to cor- 
rect for any of the nonideal characteristics of the machine or its associated 
power inverter. Among this second class of techniques (the most pertinent 
with the present contribution) is the work by Jahns [19], which proposes com- 
pensation of the disturbance effects through high bandwidth speed feedback. 
Selective elimination of torque ripple harmonics, through proper injection of 
harmonics of suitable orders in the current profiles, was first proposed in [16] 
and fnrther extended in [4], where an iterative procedure is worked out to 
eliminate higher order harmonics in the torque. In [12] and [14] the torque 
disturbance minimisation is treated as a constrained optimisation problem: 
the current profiles that minimise suitable functionals (related to the ohmic 
losses in the windings) and guarantee elimination of the torque harmonics up 
to a predetermined order, together with satisfaction of other constraints (such 
as Y connection of the phases), are computed, based on the known harmonics 
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of the back EMF profiles. Inversion of the back EMF profiles of the single 
phases (computed by means of finite element analysis) is proposed in [3]. Re- 
cently, Holtz and Springob [13] have proposed a self-commissioning control 
scheme which identifies the machine parameters and adaptively controls the 
current using a standard microcontroller. 

2.3 Torque Disturbance Model 

As it is shown in [5], the following relation can be used to represent in a 
compact form the effects of the disturbances on the torque production: 

r = r(a, I ) = 7(a) + Kj{l + 6{a)). (2.1) 

The term 7(a) accounts for the disturbances due to the cogging torque and 
to the current offset in the drives, while the second term is responsible for 
the nominal torque (with 5(a) = 0) and for the disturbances related to the 
harmonic content. It is also possible to include in 5(a) the effects of the 
amplitude imbalanee and the phase misalignments of the eurrent and back 
EMF shapes profiles [5]. 

The functions 7(a) and 5(a) in (2.1) are time-invariant, periodic with 
period 27 t with respect to the electrical angle and null in nominal conditions. 
Notice that the formulation of the model does not require knowledge of the 
characteristics of the single phases (in terms of back EMF profiles, imbalance 
and misalignments), but only of their combined effeet in the parameters of 
the sealar equation (2.1). 

Fourier expansions of the funetions 7(a) and 5(a) are introduced in order 
to parametrise the model, in the following form: 

l{a) ^ Ik siii{ka + /3k) (2.2) 

5(a) = ^ 6ksm(ka + ipk) (2.3) 

keNs 

where Nj and Ng are the sets of harmonies to be considered in the expansions 
of 7(a) and 5(a), respectively. A validation of the model, performed through 
several static measurements of the torque of a motor mechanically constrained 
in different positions and subject to different current references, has been 
deseribed in detail in [5]. 

2.4 Spectral Identification of the Disturbance 

For the purpose of online compensation, the harmonics of the torque must be 
obtained from the motor through simple motion experiments. With reference 
to expressions (2. 2, 2. 3), the goal of the identification is therefore to give 
estimates for the amplitudes 7fc and 6k and phases /3k and 'tpk- The basic idea 
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is to get them by examining the output of the position controller in a closed 
loop experiment, where a constant velocity motion, at a very slow velocity, 
is commanded. 

Consider first the block diagram (Figure 2.2) of the motor controlled 
by a linear regulator, where the disturbance is represented as an external 
input (actually, according to (2.1), it depends on a and /), while G(s) and 
-R(s) stand for the transfer functions of the process and of the regulator, 
respectively. 




Figure 2.2. Position controlled motor with torque disturbance 



2.4.1 Identification of 7 (cr). A practical way to cope with the identi- 
fication problem is to realise that the amplitude of the disturbance term 
proportional to the current reference KtI6(a) becomes actually comparable 
to the amplitude of the other disturbance term 7 (a) only when the current 
reference I takes high values, say more than 30% of the value correspond- 
ing to the nominal torque. Since in a constant and low velocity motion the 
main reason for the torque to take high values is to counteract a constant 
load (such as the gravity load or a particularly high friction term), experi- 
mental configurations have to be found, where these terms are minimised. In 
these configurations, the torque disturbance can be entirely ascribed to the 
current-independent term 7 (a), with quite good approximation. 

The following relation, in terms of transfer functions, is easily obtained: 



Kj = 



KtR{s) _ L{s) ^ 
1 + T(s)^'" 1 + L{sf 



(2.4) 



where L{s) = KtR{s)G{s), and 



G{s) 



1 



with Jm and Dm being the inertia and viscous friction coefficient of the motor 
respectively. Inverting (2.4) one obtains: 



d = 



1 



1 -|- i(s) 



Kj 



(2.5) 
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and taking the following approximation for the closed loop transfer function 
of the system: 

Hs) ^ 1 

1 -^( 5 ) 1 “t“ 

with LOc being the crossover frequency of the loop, (2.5) can be rewritten as 
follows: 

d — d" dD^s)qrn (1 d” S /iOc)d^td 

finally yielding, in the time domain: 

d = JmQm + DmQm ~ ~ —I- (2.6) 

UJq 

The identification thus goes like this: assuming the position reference 
a known function of time, its first two derivatives are computed at each 
sampling instant, and the samples of the current reference commanded by 
the position regulator are stored. The time derivative of this signal is then 
computed numerically, while (2.6) gives the time series of the estimates of the 
disturbance. Since d = 7(0), Fourier analysis of this time series with respect 
to the stored position measurements of the angle g™ provides estimates of 
the amplitudes and phases of the harmonics of 7(0). 

Notice that the time derivative of the current reference required by (2.6) 
can yield noise amplihcation. If this is the case, this latter term can be ne- 
glected, provided that the spectrum of the position reference signal is entirely 
contained in the bandwidth of the closed loop system. 

2.4.2 Identification of 6{a). For the identification of the function 6{a), a 
second experiment has to be performed, in a configuration where a significant 
static load is present, so that the contributions of the terms 7(0) and ^(a) to 
the disturbance are actually comparable. The disturbance d is still computed 
as in (2.6), but now it is assumed: 

d = 'j{a)+Kj6{a). 

Fourier analysis of the function d derived from the above relation, while using 
the approximation of 7(0) already known from the previous identification, 
will provide estimates of 6k and %pk- 

2.5 Adaptive Identification of the Disturbance 

While the procedure described in Section 2.4 solves the identification prob- 
lem with a modest computational effort, it is still a batch method, where 
the set of data required to compute (2.6) is first obtained, perhaps before 
normal operating condition of the motor, and then the result of the spectral 
identification are applied for compensation of the disturbance. 

However the first harmonic of the current independent component j{6) 
shows typically a time dependence [6] , due to a thermal drift of the offset in 
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the current sensors. In order to cope with this and other time dependences, 
an adaptation of the estimates is to be implemented. Here the procedure for 
an adaptive estimation of the first harmonic of the disturbance 7(a) will be 
shortly outlined. 

The idea is simply to find the estimates for amplitude 71 and phase /3i of 
the sinusoid by minimising the following quadratic loss function: 

N 2 

J = ^ - 7i sin + /3i) 

1=1 

where N is the total number of data used for the identification and d is 
evaluated as in (2.6). Explicit formulas for the estimates of 71 and (3\ can 
be easily obtained with this minimisation. A recursive algorithm can then be 



setup in the form: 
0i{i) 


= /3i(i- 1) +/^ l),d(i)) 


(2.7) 


71 (0 


= 7i(i - l)+fj (ji{i - . 


(2.8) 



Details on functions fp and can be found in [9]. 



2.6 Design of a Compensation Technique 

Once the estimates for functions 7(0) and 6{a) have been obtained, either 
from the spectral analysis outlined in Section 2.4 or the recursive least squares 
algorithm sketched in Section 2.5, a compensation scheme as in Figure 2.3 can 
be setup. Let 1° be the current reference determined on the basis of a model 




Figure 2.3. Position controlled motor with compensation 
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of the motor free of the disturbances. In other words, let 1° = fjKt, where 
f is the torque that the motor is supposed to deliver. The actual current 
reference will be determined as follows: 

1 + ^(a) 

This way, in nominal conditions (5(o;) = ^(a), 7(a) = 7(0;)) the effect of the 
disturbance is completely eliminated. 

Thus the estimates 7(0;) and ^(a) are used to continuously (i.e. at every 
sampling instant) modify the current reference 1° produced by a closed loop 
controller. 




Angular frequency (rad/s) 




Figure 2.4. a) Amplitudes of the harmonics of the motor velocity; b) Time history 
of the velocity 



2.7 Experimental Results 

Experiments have been made on an ECS (Electronic Control Systems) 105ES 
30022 PMAC BLM (nominal torque 2.2 Nm, 4 pole pairs), instrumented 
with a resolver, and coupled to a ISODRIVE 325 drive (by ECS), perform- 
ing current control. The motor position control is performed through a PC, 
implementing a P/PI algorithm, with a sampling time of 512 /is, as well as 
the identification algorithm. Embedding the software in the control hardware 
of the drive would require a memory storage which is estimated in about 2 
Kbytes, and the computation of the trigonometric functions needed for the 
spectral analysis. 

In the experiments the motor was free from external loads (including 
gravity): this means that only the current-independent term 7(a) was of 
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interest. Moreover, from the results of the spectral identification it became 
evident that only the harmonics corresponding to k = 1,6 could have been 
retained from the complete expansion (i.e. = { 1 , 6 }), without considerable 

loss of accuracy. 

To assess the effectiveness of the proposed compensation scheme, an ex- 
periment has been run twice, without compensation and with the compensa- 
tion on respectively, commanding a 15 rad/s constant velocity motion. The 
motor velocity, derived from the position measurements, has been Fourier 
analysed with respect to time before and after compensation, for validation 
purposes. The amplitudes of the harmonics are compared in Figure 2. 4. a, 
where it is apparent that the harmonic at 60 rad/s (corresponding to A: = 1 ) 
has been reduced to about 15% of its value without compensation. The other 
harmonics are left almost unchanged. Figure 2.4.b reports two consecutive 
records of the time histories of the velocities, the first one without compensa- 
tion, the second one with compensation. Again it is apparent that the most 
evident first harmonic (whose period amounts to 27 t/ 60 = 0.1s) is almost 
suppressed. 





Figure 2.5. a) Evolution of the estimate for a constant velocity motion; b) Time 
history of the velocity 



The recursive algorithm (2.7), (2.8) has then been used to implement an 
adaptive compensation of the first harmonic of the 7 (a) disturbance. In the 
first part of the experiment, the compensation is kept off and the recursive 
algorithm works to update the estimates of the amplitude and phase of the 
disturbance. When these estimates reach a steady value, the compensation 
is switched on, while the recursive algorithm keeps updating the estimates. 

First an experiment with a constant 3 rad/s velocity motion has been run. 
The evolution of the estimate for the amplitude of the harmonic is shown in 
Figure 2. 5. a (the estimate of the phase follows a similar evolution). After 
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about 3 seconds the estimate reaches a steady value and the compensation is 
switched on. Figure 2.5.b reports the motor velocity, as numerically computed 
from the position measurements. The role of the compensation in suppressing 
the disturbance harmonic is quite evident. 

A second experiment with a sinusoidal velocity reference has been run. 
Again, the estimate of the amplitude of the disturbance (Figure 2. 6. a) con- 
verges after about 3 seconds (the same for the estimate of the phase). The 
motor velocity plotted in Figure 2.6.b emphasises again the reduction of the 
disturbance. 





Figure 2.6. a) Evolution of the estimate for a sinusoidal velocity motion; b) Time 
history of the velocity 



3 Modelling and Compensation of Load Oscillations 

3.1 Model of an Elastic Servo Mechanism 

A fast dynamics servo may have several flexible elements and connections, 
resulting in a dynamic behaviour affected by several resonances. From the 
point of view of the control design, it is essential to model correctly the 
lowest frequency resonance, which play an essential role in the design, as it 
is shown later on. Under certain conditions that are frequently satisfied, the 
hrst resonance frequency is correctly predicted by the well known model of 
two masses connected through an elastic element (Figure 3.1). The model is 
described by the following linear equations [27], [31], if the non linear friction 
terms are neglected: 

— 

'^m — 



Ktl 

JmQm Djyi Qm +Tt 



(3.1) 

(3.2) 
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Figure 3.1. Elastic servo positioning system 



Tt = Kei{qm-nqi) + Dei{qm-nqi) (3.3) 

UTt = Jiqi (3.4) 



where Tm is the motor torque, I is the motor current (actually the amplitude 
of the sinusoidal current of each stator phase of the brushless motor); Kt 
is the motor torque constant, qi and qm are the load and motor angular 
positions, respectively; J/, Jm are the load and rotor inertia respectively; Dm 
is the viscous friction coefficient at the motor side; K^i, D^i, are the joint 
stiffness and damping factors, respectively; n is the transmission ratio; riTt is 
the torque delivered by the gear reducer at the load side. Furthermore, thanks 
to the fast dynamics current amplifiers, the difference between the current 
command I (control variable) and the actual current I can be neglected (/ = 
I) for the purpose of position control design. Pulsating torque disturbances 
act as additive, variable frequency disturbances to be rejected by the control 
system. The transfer function Gym{s) from the motor torque to the motor 
velocity is then given by: 






JlrS^ + DgiS + Kgi 



(3.5) 



with 



Z\(s) — JlrJm^ 4” [(d/Ti 4“ Jlr') D^i + JlrDm] S 4" 

4- [{Jm 4- Jlr) Kel 4" DmDyi] S 4" DmKel 



where Jir = Ji/nJ is the load inertia reported to the motor side. 

The polynomial A{s) does not have a simple factorisation but, for common 
values of the system parameters, it shows a couple of complex roots whose 
natural frequency and damping factor can be approximately obtained letting 



= 0 : 



JlrJm 






J. 



eff 



Jlr Jm^ el 



where J^g = Jm + Jir- The third pole Sr is in a lower frequency range, as it 
is strongly related to the rigid behaviour of the system {sr = —Dm/ Jeff)- 
The zeros of the transfer function are complex too, with natural frequency 
and damping factor given by: 
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It is worth noting that 

^ = f^ = ^/TT7>i 

p = Jir/ Jm being the inertia ratio. Motor inertia and transmission ratio are 
frequently selected to pursue the inertia matching, namely to obtain p = 1. 

3.2 Existing Solutions for the Compensation of Load Oscillations 

In the review work [25] the disturbance observer technique, already presented 
in [30] and [29] , is used to control a rigid servo affected by an unknown load 
torque, while the resonance ratio control is used when an elastic transmission 
is considered. The disturbance observer is also used in [15], [32], where tor- 
sional flexibility of the joint is not directly accounted for. A review of several 
works dealing with suppression of torsional oscillations is reported in [31], 
where the contributions are divided in three categories: 1) methods that ex- 
ploit measurements on both the motor and the load sides; 2) methods based 
on the motor measurements only and observation of the state of the system; 
3) notch filters plus conventional loopshaping techniques. The first category 
include most of the works where nonlinear control theory is used: singular 
perturbation and integral manifold theory is used in [22] for a single joint case 
and in [27] for the extension to a complete articulated manipulator; feedback 
linearisation is used in [8] ; nonlinear observer theory in [28] ; adaptive control 
theory in [18]. The second group includes more industry-oriented works, such 
as [17] and [26], as well as the early work [23], where a PD controller on the 
motor coordinate, enforced with a compensation of the gravitational effect, 
is considered. Finally, the analysis in [31] mostly belongs to the third group. 

3.3 Classical Control and Load Oscillations 

The control system requirements concern setpoint tracking and rejection of 
load and torque disturbances. Both of them demand for large control system 
bandwidth, namely for large feedback loop gain. However, a critical problem, 
especially during slow motion and at motion starts and stops, is due to load 
oscillations. For instance, load oscillations may reveal as vibrations of the 
end effector of a robot arm during the execution of a slow motion, like in 
arc welding tasks, while in milling and grinding machines they may cause 
some slight undulation, and thus poor finishing, of working surfaces. Since 
the analysis of the properties of the elastic model shows that high feedback 
gains on motor position increase load oscillations, a trade off is needed on 
the loop gain. 
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In most commercial products, position control (Figure 3.2) consists of a 
position loop with a proportional (P) regulator, implemented for instance in 
a CNC, cascaded with a velocity loop with proportional-integral (PI) regu- 
lator, implemented in the drive electronics. The integral action ensures that 
the error on motor position for constant setpoint, load torque and torque dis- 
turbances (rd), vanishes at steady-state. While in the past the velocity was 
sensed by a tachometer, in current products it is obtained by numerical dif- 
ferentiation of the motor position, sensed by either an encoder or a resolver. 
To improve the setpoint tracking capabilities, a feedforward derivative action 
is also inserted from the position setpoint to the velocity one. 

It is easy to check that this control scheme is equivalent to a PID controller 
with real zeros, fed with the motor position error Cp = qm — Qm and with 
output I. 




Figure 3.2. P/PI control with feedforward action 



Note that in the block diagram of Figure 3.2 a block has been added, with 
transfer function: 



r' ( \ 1 ^elS + Kel 

Crlm{s) = -- 



n JirS'^ + Deis + Kel ’ 



(3.6) 



which allows computation of qi from q^n- 

The velocity loop is designed first. The loop transfer function is: 



T^(s) — KeyKiJij- 



(s -|- 1/Tiy) (s^ -|- 



sZ\(s) 

and the closed loop transfer function from setpoint to motor velocity is: 

(s -|- 1/Til,) (s^ + ‘2^z^zS + 



Tu(s) — KeyKfJie 



Z\„(s) 



where Ay{s) is defined as 

2iu(s) = JlrJmS -f- [(Tm -t- Jlr) Del T JlrDm T KevKfJie\ S -\- 

+ [{Jm + Jlr) Kel + D^Del + KcvKtJlr (2^z0;2; -|- 1 /Ti„)] -|- 

+ [DmKel -P KcvKtJlr + 2^2Wz/Tii,)] S -p KcvKtJirUj'^/Tiv. 
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For Kcv 00 the system will be stable, whatever Tiy > 0, but two poles 
approach the lightly damped process complex zeros. In this case, also two 
roots of the characteristic polynomial of the closed position loop, given by: 

^p(s) = SAy[s) + KcpKcyKtJlr (s + 1/Tiy) 

become the process zeros, whatever Kyp > 0. On the other hand, for smaller 
values of Kyy two of the position loop poles become the process zeros only 
for Kyp 00 . 

If two closed loop poles are equal (or almost equal) to the process zeros, 
the relevant dynamics is not observable from the motor position, but it reveals 
with oscillations of the load, because of the poles of Gim{s) (3.6). 

As a conclusion, the controller design requires to trade off between higher 
gains (in particular Kyy), that increase setpoint tracking and torque distur- 
bance rejection capabilities, and lower gains, needed to keep the damping of 
closed loop poles reasonably high. 

In this respect, the common industrial practice of tuning the velocity 
loop as fast as possible, increasing the gain Kyy until an “audible noise” is 
generated by the motor, proves to be quite dangerous. In fact, this is exactly 
the way to induce load oscillations, as the high gain velocity feedback places 
two closed loop poles near the lightly damped process zeros, where they 
remain also after closing the position loop. 

3.3.1 An Illustrative Example. The prototype servo positioning system 
of Figure 3.3 is considered as an illustrative example. 




It consists of a Control Techniques permanent magnet AC brushless motor 
(PMAC BLM), with a nominal torque of 2.3 Nm and a nominal power of 700 
W, a Harmonic Drive speed reducer, and a load. Since the load rotates in a 
horizontal plane the gravity force does not act on the system. This system is 
controlled by a PC as described in Section 2.7. The parameters of the testbed 
are given in Table 3.1 (in SI units). 
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Kt 


Jm 


Ji n 




Kci 


Del 


1.6 


1.5 X 


10”'* 


2.7 100 


3.4 X lO”** 


3.05 


2.2 X 10”** 


OJz 


1 


U 


(jJp 




Sr 


Tz 


106.3 


bo 


X 10” 


^ 177.5 


0.105^0.064 


8.13 


7.2 X ir** 



Table 3.1. Physical parameters 



Figures 3. 4. a and 3.4.b show the root loci of the position loop at vary- 
ing Kcp, obtained with Tj„ = l/|sr| (low frequency zero-pole cancellation) 
and with two different choices of Kcv, respectively K^v = 5.2 x 10“^, corre- 
sponding to a fast but poorly damped velocity loop, and Kcv = 1-84 x 10“^, 
corresponding to a slower but well damped loop. In the first case, two poles 
of the position closed loop are very near the process zeros, even for low values 
of Kcp, while in the second case the poles starting from the complex poles of 
the velocity closed loop go toward infinite. The value of Kcv discriminating 
this behaviour, in this example, has been found to be Kcv = 2.4 x 10“^, 
corresponding to ojcv = 93.78, i.e. Ucv = 0.52wp = 0.88w2, which resembles 
the well known rule of thumb for the choice of u>cv, namely oJcv < 0.5ct>p. 

The time responses of qm and nqi to steps in setpoint and torque distur- 
bances for both cases, computed with Kcp = 30 (i.e. approximately Wcp = 30) 
and without the feedforward derivative action, are given in Figures 3. 5. a 
and 3.5.b respectively. The response to torque disturbances is dominated by 
the real pole canceled by the zero of the PI regulator. If the cancellation is 
avoided, this mode can be made faster. The disturbance rejection is stronger 
for Kcv = 5-2 X 10“^ but the load behaviour is more oscillatory. 



3.3.2 On the Use of the Notch Filter. Commercial drives frequently 
enhance the P/PI control scheme using a notch filter, namely a filter with 



transfer function: 



^ nf (^) — /r 



-|- uy^ 



Sometimes the filter is inserted within the velocity loop, in series to the PI, 
sometimes outside. In principle, the filter can be used to avoid the excitation 
of oscillatory modes through the control variable. However, in industry there 
are controversial opinions about the effectiveness of the notch filter and on 
the best way to exploit it. Frequently, people rely on the notch filter to coun- 
teract oscillations which cannot get rid of otherwise. It is also suggested to 
use the notch filter to cancel the open loop complex poles and replace them 
with more damped poles [31]. Precise cancellation however is difficult because 
of uncertainty on the servo system parameters, especially the damping coef- 
ficient. Moreover, poles are canceled in the transfer functions involving the 
setpoint, but they remain unchanged in the closed loop and may be excited 
by other inputs, especially by torque disturbances. 

Following the above root locus analysis, it is suggested in this contribution 
to place the filter outside the velocity loop and to select its zeros so as to 
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cancel the poles of the velocity loop. The position loop root locus, for the 
case Kcv = 5.2 x 10“^, becomes as in Figure 3.6. 




Figure 3.6. Root locus of the position loop with P/PI plus notch filter 




Figure 3.7. Time responses of qm (dashed line) and nqi (solid line) for Kcv = 
5.2 X 10“^ with the notch filter 



The closed loop dominant poles, again with Kcp = 30, are now well 
damped, even if the poorly damped poles of the velocity loop are still observ- 
able in the response to torque disturbances. As a result, the step responses 
of qm and nqi are as in Figure 3.7. There is a clear improvement in the re- 
sponse to the setpoint (the overshoot is due to the step input, which must be 
avoided) and a smaller one in that to the torque disturbance, whose dynamics 
is dominated by the canceled poles. 
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As a conclusion, designing jointly the velocity regulator and the notch 
filter, a good trade off can be obtained between velocity loop bandwidth and 
damping of the closed loop dominant poles. The velocity loop gain Kcv should 
be first chosen to get a desired damping of the velocity loop poles. Then, these 
poles are canceled (i.e. blocked) by the notch filter. Finally, the position loop 
gain Kcp is selected to achieve the desired damping of the dominant poles of 
the servo system. 

The filter design proved also to be robust to errors in model parameters, 
since the notch filter zeros cancel out closed loop poles (the velocity loop 
ones), which are less sensitive to process parameter variations, than the open 
loop ones. 



4 LQG Load Position Control 

Considering the limitations of classical control with respect to the load be- 
haviour, it is sensible to investigate solutions which take into account the 
whole state of the system under control, so as to completely assign the inter- 
nal dynamics of the closed loop system, removing the lightly damped eigen- 
values. The setpoint response of a particular state (the load position, in our 
context) will then be shaped through suitable feedforward actions. 

Consider then the block diagram of Figure 4.1. The state-space plus 




Figure 4.1. A state-space controller plus feedforward controller 



feedforward controller is made up by the following functional blocks: 

— Setpoint generator for the load position (not shown in figure). 

— Setpoint conversion (from load to motor position). 

— Feedforward for setpoint tracking. 
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— Integral term on the motor position error. 

— Full order state observer or Kalman filter. 

— State-space control law. 



4.1 Design of the Optimal Controller 



In state space form, model (3.1)-(3.4) is described as follows: 

X = Ax + Bu (4.1) 

y = Cx (4.2) 

with X = [ qm q-m qi qi , B = [O Kt/Jm 0 0 ]^, w = 7, C = 
[ 1 0 0 0 ] (the motor position being the only measured variable), and: 




0 

K^i 
J m 

0 



1 0 

Dm + Del nKel 
J m Jm 

0 0 

Jl Jlr 



0 

nPel 
J m 
1 

_ Del 
Jlr 



The integral term on the motor position error is needed to eliminate 
steady state errors for constant disturbances, such as Coulomb friction. The 
gain ki of the integral term is computed together with the matrix gain K 
of the state space control law, solving an optimal control problem. Denoting 
with xi the state of the integral term, reference is thus made to an augmented 
state vector x = [ x’^ Xi ], whose dynamics are given by: 



X = Ax -f Bu + Gqm 

with 0],G=[0 0 0 0 l]^, while: 



i = 



A 

—c 




As is well known, thanks to a separation principle [1], it is possible to 
carry out the design of the state space controller as if the whole state were 
available, postponing the problem of how to design a suitable dynamic system 
to get a correct estimation of the state. The quadratic loss function for the 
optimal LQ problem is: 

J = / [x{t)'^Qx{t) + dt 

Jo 

The weight matrix Q is given the expression: 

Q = pDD'^ 

where p is a scalar, while 7) € 7?® is a vector to be determined in such a way 
that the following transfer function: 
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(where po(s) is the open loop characteristic polynomial of matrix A) has 
some desired properties. Specifically, it can be proven [1] that, if m(s) is a 
fourth order polynomial, as p approaches infinity, four roots of the closed 
loop polynomial approach the roots of m(s), while the remaining fifth root 
moves along the negative real axis away from the origin of the complex plane. 
The guideline to select the vector of weights D is thus to place the roots of 
polynomial m{s) in desired positions of the complex plane (since four of the 
closed loop roots will approximately coincide with them). Then the scalar p 
is chosen as the minimum value needed to have the above asymptotic result 
hold approximately true. Values of p higher than necessary should be avoided, 
since they entail an excessive use of the control variable, as it is apparent from 
the expression of the loss function. Moreover, the crossover frequency of the 
control loop turns out to be proportional to y^. 

As an example, for the prototype servo-positioning system described in 
Section 3.3.1, the roots of m{s) have been chosen as in Figure 4.2 (two couples 
of complex roots, both with damping 0.7, one with natural frequency 80 rad/s, 
the other one with natural frequency 230 rad/s). Selecting p = 10®, four of 
the closed loop eigenvalues are positioned on the roots of m(s) while the 
remaining one is placed on the real axis in the point shown in Figure 4.2. 




Figure 4.2. Roots of the polynomial m[s) 



The selection of the weights in the loss function (the ever present problem 
of an optimal control design) has thus been converted to a somewhat partial 
pole placement. However, compared to standard pole placement design, the 
method offers as a byproduct a way to assess the validity of the choice made 
on the closed loop eigenvalues, given the open loop eigenvalues. Very high 
values of p are symptomatic of a bad choice, both in terms of control effort 
needed to impose the new desired dynamics to the system and in terms of 
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sensitivity of the closed loop eigenvalues to small errors in the parameters of 
the model. As an example, placing all the eigenvalues for our problem in the 
same point of the real axis (for example around —120 rad/s) needs a value of 
p three orders of magnitude higher and results in a dramatic increase of the 
sensitivity of the design to model uncertainties. 



4.2 Design of the Kalman Filter 



The design of the state observer has been carried out in a stochastic frame- 
work, using Kalman filter theory. Noise inputs have been added to the system 
model (4.1), (reffmrSS2) as follows: 

X = Ax + B[u -\- u] 
y = Cx + w 



where u is a torque disturbance, while to is a disturbance on the measure. 
The noises are assumed to be uncorrelated, zero mean, white Gaussian noises, 
whose (scalar) variances are in fact all is needed for Kalman filter design. 

As far as the variance of the noise on the output measure is concerned, 
the following formula has been used, which refers to the quantisation noise 
of the A/D converter: 



cr 



2 

W 



K 

12 



1 

12 




1 

3 



2-2h^2 



where he is the quantisation step of the converter, b is the number of bits 
used in the converter, Ac is the input range of the converter. 

A similar formula could be used for computing the variance of the input 
disturbance, based on the quantisation of the D/A converter. However fine 
tuning of this value proved to be essential to get satisfactory results from the 
design of the filter. 



4.3 Design of the Feedforward Filters 

The last step in the design of the control system concerns the two open loop 
filters: the conversion of the load position setpoint to the motor position 
setpoint (this is needed to form the error which feeds the integrator) and 
the feedforward filter which shapes the setpoint response of the load. Let 
Gr{s) and G/(s) be the transfer functions of the two filters, respectively 
(Figure 4.3). 

The transfer function Gr{s) should ideally be the inverse of the transfer 
function Gim{s)- 

On the other hand, the transfer function Gf{s) should additionally invert 
the transfer function Gk{s) of the system obtained feeding back the servo po- 
sitioner with the state space control law, operating on the observed state (see 
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Figure 4.3. Design of the feedforward filters 



Figure 4.3). It is straightforward to realise that the dynamics of the observer 
do not contribute to this transfer function, which takes the expression: 

Gfc(s) = C[sI-{A + BK)]~^ B 

and its two zeros are the same as the zeros of the open loop transfer function 
(3.5) from I to qm- 

In order to implement the two open loop filters with causal operators, the 
following choice is made: 

Gris) = Ggis)-^F{s) (4.3) 

Gfis) = Gkis)-^Gg{s)-^F{s) (4.4) 

where F{s) is a transfer function whose relative degree is at least equal to 
3. Notice that this constraint is essential for the causality of (4.4). However 
introducing in (4.3) the same filter as in (4.4) allows concluding that the 
nominal transfer function from the load position setpoint to the load position 
is simply F{s). 

For example, F{s) could be chosen as: 



(1 + s/u;0)(i + 2C0sM + ^VK)^) 

For the same design of the control system characterised by the choice of the 
closed loop eigenvalues shown in Figure 4.2, u>p was set to 350 rad/s, to 
800 rad/s, C° to 0.7. 
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4.4 Experimental Results 

The control scheme described above has been tuned and verified on the proto- 
type servomechanism described in Section 3.3.1, and its performance has been 
assessed by comparison with a standard PID controller on the motor position 
error, tuned to get the maximum available bandwidth (about 80 rad/s). 

Three experiments will be shown here, all of them characterised by a 
trapezoidal profile of the reference velocity, but with different parameters, as 
reported in Table 4.1. Figures 4.4 to 4.6 compare the load velocities with the 
two control schemes, obtained from an accelerometer placed on the load for 
validation purposes. 



Experiment 


1 


2 


3 


Load rotation (deg) 


40 


30 


50 


Time for positioning (s) 


0.5 


0.5 


0.9 


Maximum acceleration (rad/s^) 


1200 


900 


500 



Table 4.1. Parameters of the experiments 




Figure 4.4. Load velocity in experiment 1 



In the first experiment (where the fastest positioning has been com- 
manded), the benefits of the LQG controller in terms of suppression of the 
load oscillations are evident: the large overshoot obtained with the PID con- 
troller almost disappears. Since this was the primary goal of the state-space 
controller, it can be concluded that the result of the implementation of the 
LQG has been successful. On the other hand, experiment 2 and, above all, 
experiment 3 (the slowest positioning) exhibit high frequency oscillations of 
the load with the LQG controller. These have been ascribed to noises, whose 
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Figure 4.5. Load velocity in experiment 2 



frequencies are proportional to the motor velocity, such as the torque ripple 
on the brushless motor, discussed in Section 2.3, and the ripple on the motor 
position transducer [11]. These noises enter the Kalman filter, where they are 
amplified, and their effect is visible on the observed load velocity. 




Figure 4.6. Load velocity in experiment 3 



5 Conclusions and Future Directions 

Modelling and control design of an elastic servo positioning drive and mod- 
elling, identification and compensation of the pulsating torque disturbances 
in PMAC BLM have been discussed in this paper. By feeding back only the 
motor position it is relatively easy to obtain satisfactory control of the mo- 
tor position and velocity. Nevertheless this does not guarantee a satisfactory 
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control of the load position and velocity for demanding motion control appli- 
cations. This happens especially during slow motion, when the frequencies of 
the pulsating disturbances generated by the motor and by the gear reducer 
match, and thus the servo structural resonance is excited. 

It is shown how to design the widely used P/PI control to achieve a 
balanced trade off between a fast motor response and a damped behaviour of 
the load position. This helps matching the strong requirement of a smooth, 
non oscillating, slow velocity motion, typical of robotics, machine tools and 
other applications, but also points out the limits of P /PI control. P /PI control 
happens to be rather robust with respect to system parameter variations but 
its performance may not be fully satisfactory in those applications where the 
load velocity ripple is a major problem. To this purpose a new way to exploit 
a notch filter has been proposed, not only to prevent from harmonic signals 
(at the notch frequency) to enter the velocity loop but also to improve the 
damping of the closed loop (dominant) poles. 

Modern control theory has been then used to design a LQG plus feed- 
forward controller: advantages with respect to classical control seem to be 
achievable for fast positioning manoeuvres, but the increased complexity of 
the implementation makes this choice still questionable for industrial use. 

In addition to this, the reduction of the torque pulsation at source level 
is the most promising approach to the suppression of load oscillation at low 
velocity, to our experience. Therefore a compact model of pulsating torque 
disturbances suitable for online compensation has been given, together with 
algorithms for offline and online identification of the motor parameters. Ex- 
periments have been given which show the effectiveness of this approach for 
disturbance compensation. 
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In this chapter, we consider problems that arise in designing, building, plan- 
ning, and controlling operations of robotic hands and end-effectors. The pur- 
pose of such devices is often manifold, and it typically includes grasping and 
fine manipulation of objects in an accurate, delicate yet firm way. We survey 
the state-of-the-art reached by scientific research and literature about the 
problems engendered by these often conflicting requirements, and the work 
that has been done in this area over the last two decades. Because of space 
limitations, the chapter does not attempt at providing a survey of the technol- 
ogy of robot hands, but rather it is oriented towards covering the theoretical 
framework, analytical results, and open problems in robotic manipulation. 



1 Introduction 

In many roboticists, the admiration for what nature accomplishes in every- 
day’s functions of human beings and animals is the original stimulus for their 
research in emulating these capabilities in artificial life. Among the many 
awesome realisations of nature, few of the human abilities distinguish man 
from animals as deeply as manipulation and speech. Indeed, there are animals 
that can see, hear, walk, swim, etc. more efficiently than men — but language 
and manipulation skills are peculiar of our race, and constitute a continuing 
source of amazement for scientists. In this chapter, we will consider in detail 
the implementation of artificial systems to replicate in part the manipulating 
ability of the human hand. 

The three most important functions of the human hand are to explore, 
to restrain, and to precisely move objects. The first function falls within the 
realm of haptics, an active research area in its own merits [48]. We will not 
attempt an exhaustive coverage of this area. The work in robot hands has 
mostly tried to understand and to emulate the other two functions. We will 
distinguish between the task of restraining objects, sometimes called grasping 
or fixturing, and the task of manipulating objects with fingers (in contrast to 
manipulation with the robot arm), sometimes called dexterous manipulation. 

While grippers and fixtures have been used extensively in industry, one 
can argue that the field of robot grasping started with the work of Asada and 
Hanafusa [4] and Salisbury’s first three- fingered robotic hand [61]. Since then. 
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many hand designs have been proposed, ranging from rather simple devices 
to very sophisticated multifingered hands such as the Utah-MIT hand [41]. 
Extensive surveys on robot hand systems are for instance those reported in 
[31], [37], [71], [94], and more recently [1], [75], [7]. 

In robot hand design, it can be observed that there are two prevailing 
philosophies, which can be identified with an anthropomorphic vs. a mini- 
malistic approach to design. While the former philosophy basically attempts 
at replicating the human hand capabilities by imitating its mechanical struc- 
ture, the latter focuses on realisation of some desirable grasping or manipu- 
lation features by purposeful design of mechanisms that have no intentional 
resemblance with any biological system. In the latter group, there have been a 
number of efforts focussing on reduced-complexity multifingered hands. Two 
examples of robot hands inspired to the two approaches and developed by 
groups participating in the RAMSETE project are reported in Figures 1.1 [64] 
and 1.2, respectively. 




Figure 1.1. The University of Bologna dexterous hand 



Design of robot hands still poses many challenges to the research commu- 
nity, and several are common to the two approaches above. However, it seems 
fair (though perhaps slightly oversimplifying) to affirm that anthropomorphic 
design is mostly confronted with technological problems such as accuracy and 
miniaturisation of sensors and actuators, power and signal transmission, etc. 
In minimalistic design, instead, the emphasis of current research is more on 
the theoretical analysis of manipulation systems, and their deep understand- 
ing in order to allow full exploitation of limited hardware capabilities. This 
chapter is more focussed on the latter class of problems. 

Hardware complexity reduction can be achieved in several ways. For in- 
stance, when grasp robustness is considered, it can be observed that envelop- 
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Figure 1.2. The University of Pisa dexterous gripper 



ing grasps are superior in terms of restraining objects. Enveloping grasps 
[102], in contrast to fingertip grasps, are formed by wrapping the fingers 
(and the palm) around the object. Indeed, this is easily seen also in human 
grasping, where fingertips and distal phalanges are used in fingertip grasps 
for fine manipulation, while the inner parts of the hand (palm and proximal 
phalanges) are used in enveloping grasps for restraint [20], [40]). One of the 
first attempts at realising a reduced-complexity gripper was a three fingered 
hand powered by four actuators [104] that was designed to grasp by envelop- 
ing. Variations of this basic theme are also seen in grippers designed for the 
so-called whole arm grasps [89] and power grasps [67] . On the other hand, for 
achieving dexterous manipulation with a simplified hardware, the purpose- 
ful introduction of nonholonomic phenomena in manipulation by rolling has 
been advocated, and experimentally demonstrated, by several authors (see 
e.g. [19], [53], [71], [10]). Different modalities of manipulation and grasping 
share some fundamental theoretical framework, analytical results, and open 
problems, that are the subject of this chapter’s survey. 



2 Kinematics of Manipulation 

The model of the hand we assume is comprised of an arbitrary number of 
fingers (i.e. simple chains of links — phalanges — connected through rotoidal 
or prismatic joints), and of an object, which is in contact with some or all of 
the phalanges. We let q denote a vector of generalised coordinates, completely 
describing the configuration of the fingers; and u = (po, Rq) G SE{3) denote 
the configuration (position and orientation) of the object. With a slight abuse 
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of notation, we also denote with q and li the elements of the tangent space 
to these configuration spaces (hence u G se(3) is the object twist). 

Contacts represent a particular kind of kinematic constraint on the allow- 
able configurations of the system, and cause most of the differences in the 
analysis of dexterous manipulation from other robotic systems. Contact con- 
straints are typically unilateral, non-holonomic constraints on the generalised 
coordinates system, written in general in the form 

C(q,q,u,u) > 0. (2.1) 

The inequality relationship reflects the fact that contact can be lost if the 
contacting bodies are brought away from each other. This involves an abrupt 
change of the structure of the model under consideration. To avoid analytical 
difficulties, it is usually assumed that manipulation is studied during time 
intervals when constraints hold with the equal sign (this is not the case in 
the study of grasping, where the study of these inequalities is crucial to un- 
derstanding closure properties). The constraint relationship (2.1) is not in 
general integrable, i.e. it cannot be expressed in terms of q and u only: in- 
tegrable constraints are called “holonomic” . Holonomic constraints between 
generalised coordinates reduce the number of independent coordinates nec- 
essary to describe the system configuration (degrees of freedom) , and can be 
assumed to be removed from the description of the system by proper coordi- 
nate substitution. Nonholonomic constraints, on the contrary, do not reduce 
the number of degrees of freedom of the system, but rather reduce the number 
of independent coordinate velocities. 

Contact kinematics is a study of the relationship between the location of 
the point of contact as a function of the relative motion of two contacting 
bodies. The first fundamental work in this area is due to Cai and Roth [16], 
who studied rigid planar bodies in point contact. They derived a relationship 
for the rates of change of the location of the point of contact as a function of 
the angular and linear velocities and accelerations of the contacting bodies. 
Montana [69] provided a more formal description of the configuration space 
associated with two contacting bodies, and derived the equations of kinematic 
contact that relate the time derivatives of contact coordinates with the rela- 
tive angular and linear velocities. These equations include terms that depend 
on the curvature of the contacting bodies. Sarkar, Kumar, and Yun [90] ex- 
tended this work to include acceleration terms. By using intrinsic geometric 
properties for the contacting surfaces, they showed the explicit dependence 
on the Christoffel symbols and their time derivatives. This set of results is 
directly relevant to dexterous manipulation [75], to the analysis of higher 
order closure properties [86], to stability analysis [36], and to manipulability 
by rolling [58]. 

To describe in more detail contact constraints that are in effect in dex- 
terous manipulation systems, consider a contact between the i-th phalanx 
and the object, occurring at time t at a point described in an inertial base 
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frame B by the vector x^. A generic point on the surface of the phalanx 
will be described, in a frame Q fixed on the phalanx, by the vector 
Note that, -^x^ G is actually bounded to lie on the surface Si (which is 
assumed regular) of the link, and therefore can be regarded as a mapping 
^Xi : fai G Ui C Si C The pair (Ui, ^Xi(-^ai)) is called a chart 

for (a portion of) the snrface Si, and the 2-vector ^Ui is referred to as the 
point coordinates on the i-th link. Orthogonal coordinates can be chosen so 
that the associated metric tensor is diagonal. A normalised Gauss frame can 
be associated with each point on the surface chart that has the origin in the 
point and is fixed w.r.t to the body so that its C axis is aligned with the 
outward pointing normal, while the x ^ axes span the tangent space. 
The orientation of the Gauss frame centered in x^ w.r.t the C) frame can be 
expressed by a rotation matrix -^Ri. Similar considerations and definitions 
hold for the object surface. 

Several types of contact models can be used to describe the interaction 
between the links and the object, among which the most common are the 
point-contact-with- friction model (or “hard- finger” ), the “soft-finger” model, 
and the complete-constraint model (or “very-soft-finger” ) . In each case, the 
constraints consist in imposing that some components of the relative velocity 
between the Gauss frames that are associated with the contact point on each 
surface, are zero: 

H, (°c , -f c,) = 0 (2.2) 

where is a constant selection matrix. Being the two frames fixed on the 
object and the phalanx, respectively, their velocities can be expressed as a 
function of the velocities of the object and of the joints as 

°Cj = Gf(°a*,u)ii 

^Ci = Ji(^a*,qj)q. 



Similar relationships hold for each contact point, and a single equation can be 
built to represent all constraints by properly juxtaposing vectors and block 
matrices to obtain 



HG^ii - HJq = [ HG^ -HJ 




(2.3) 



The matrix G is usually termed as the “grasp matrix” , or “grip transform” , 
while J is referred to as the hand Jacobian. 

One of the goals of the kinematic analysis of manipulation systems is 
to explicit the relationships between joint positions and object positions. 
Equation (2.3) can be used to this purpose (see e.g. [9]). Indeed, from (2.3), 
it is clear that the vector [ii^ must belong to a certain linear space, 

and hence that there exist three vectors i^i, V 2 , and 1^3 (whose dimensions vary 
with the problem at hand) such that every possible pair of object velocity u 
and joint velocity q that comply with the kinematic and contact constraints 
of the hand system can be written as 
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= UoJ^l + Upl^2 /n 4') 

q = Qpi^2 + Qo1^3- 

The columns of Up and those of Qp form a basis of the subspaces of com- 
patible object and joint velocities, respectively. Any object motion described 
by the coordinate vector 1/2 in the image of Up must correspond to a joint 
motion with the same coordinates in the basis Qp. The images of Qo and 
Uo represent the subspaces of redundant joint velocities and under-actuated 
object velocities, respectively. 

Note that the matrices appearing on the right hand side of (2.4) are func- 
tions of the position of the contact point on the surfaces. If the dependency 
between u, q and °a, is made explicit via the kinematics of rolling (see 
e.g. [69], [58]), explicit expressions for the joint motions that are required to 
perform a desired object motion can be obtained in principle. Notice also 
that, besides the analytical difficulties, in practice we often have the case 
that the geometry of the object is poorly known, if at all. The availability 
of contact sensors that are able to provide information on the position of 
the contact points on the phalanges is therefore necessary to attempt closed 
loop control of fine manipulation. In particular, if joint angles and contact 
points are sensed, (2.4) can be used even without information on the geom- 
etry of surfaces to control the object motion about desired trajectories by 
using generalised resolved-rate control. 



3 Grasp Closure Properties 

In order to define what grasping robustness is, the notions of form closure and 
force closure of a grasp are instrumental. These properties, first introduced by 
[84], concern the capability of the grasp to completely or partially constrain 
the motions of the manipulated object, and to apply arbitrary contact forces 
on the object itself, without violating friction constraints at the contacts. 

3.1 Form Closure 

Form-closure is the ability of a hand to prevent motions of the object, rely- 
ing only on unilateral contact constraints. A mathematical definition of the 
problem can be stated as follows 

Definition 3.1. A configuration Uq of an object is form-closed by a hand in 
configuration q i/ Uq is an isolated solution of the contact inequalities (2.1), 
i.e. if for all u close to Uq, C(q, q, u, li) > 0 ^ u = Uq. 

This purely geometric, rather general definition of form closure can be spe- 
cialised to allow easy-to-check tests. In particular, in many cases it will suffice 
to look at the first-order approximation of the contact inequalities, which un- 
der rather general circumstances can be written as 
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N^G^ii > 0 (3.1) 

where G is the grasp matrix (evaluated at the current configuration) and N 
is a matrix stacking contact normal vectors in its diagonal. Hence we have 
the following cases 

i) if there exists li such that all components of N^G^ii are positive, the 

grasp is not form-closed; 

ii) if for all li, N^G^ii has at least one strictly negative component, the 

grasp is form closed; 

hi) if case i) does not apply, but there exists li such that N^G^ii is nonneg- 
ative, the grasp may or may not be form-closed. 

In cases i) and ii), second order terms are negligible, and form closure can 
be decided by first order arguments, using for instance linear programming. 
Specifically, case ii) is termed “first-order form closure” , and it corresponds 
to the most widely studied case in the literature. On the other hand, second 
or higher order effects must be taken into account in case iii). 

First-order form closure (which also has direct bearing to the design of 
mechanical fixtures and jigs for manufacturing parts) has been stndied since 
the 19th century. Early results showed that at least four frictionless contacts 
are necessary for grasping an object in the plane, and seven in the 3D case. 
In [68] and [59], it was shown that four and seven contacts are necessary 
and sufficient for the form-closure grasp of any polyhedron in the 2D and 
3D case, respectively. An active area of research is the synthesis of form- 
closure grasps, i.e. given the object geometry, where to place contacts so as 
to prevent object motions. Constructive procedures for placing contacts on 
given objects to achieve form closure have attracted much attention in the 
literature, due also to the relevance to the fixturing problem (see e.g. the 
early work of [60], and more recently [33], [96], [11], [56], [55], [105]). There is 
also a form-closure analysis problem, i.e. given an object and a set of contact 
locations, to decide whether the object has any degree-of-freedom left, and 
which. Both qualitative (true/false) tests (see e.g. [51], [61], [68], [34]) and 
quantitative (quality index) tests [47], [101], [65] have been proposed for form 
closure. The extension of the classical, first-order notion of form closure to 
the so-called immobilisation problem, where second-order effects due to the 
relative curvature of the surfaces in contact are taken into account, has been 
introduced rather recently to provide more detailed results (see e.g. [35], [86], 
[103]) in case iii) above. 

3.2 Force Closure 

The analysis of form closure is intrinsically geometric, and does not take into 
account the kinematics and characteristics of the end-effector. While there 
is a wide consensus in the literature on the definition of form closure, the 
concept of force closure is somewhat less clear-cut and universally accepted. 
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The intuitive meaning of force closure implies that motions of the grasped 
object are completely (or partially) restrained despite whatever external dis- 
turbance, by virtue of suitably large contact forces that the constraining 
device (the end effector) is actually capable to exert on the object. 

The force and moment balance equations for an object subject to an 
external force f and moment m, while grasped by a robotic mechanism by 
means of n contact forces Pi applied at contact points c^, is written as 

w = Gp, (3.2) 

where w = is the external wrench, and p = [Pi' ... P^]^- 

The relationship between contact forces and the torques at the m joints of 
the robotic hand can be written using the hand Jacobian as 

T = J^p, 

A general solution of (3.2) can be written in the hypothesis that w is resistible 
(i.e. rank G = rank [G w]) as 

p = G^w + Ax, (3.3) 

i.e. the sum of a particular solution of (3.2) (G-^ is a right-inverse of G), and 
a homogeneous solution. A is a matrix whose column form a basis of the null- 
space of G. The coefficient vector x G 1R^° parametrises the homogeneous 
solution. Internal contact forces ph = Ax have no direct effect on the external 
wrench w, but play an important role in the robustness of the equilibrium 
with respect to slippage induced by external disturbances, by allowing to 
“squeeze” the object in the grasp. It should be noted that, in general, for 
grasping mechanisms with few degrees of freedom , it may not be possible to 
apply arbitrary internal forces (see below Section 7). 

In force-closure analysis one generally has to deal with frictional con- 
tacts. In different models of contact, such as the contact-point-with-friction, 
soft-finger, or very-soft-finger, friction forces and torques will be subject to 
limitations due to Coulomb’s law of friction or to its generalisations (see 
e.g. [30], [38]). We consider here contacts of the first type (generalisation 
poses no difficulties), for which Coulomb’s inequality holds, 

o-*,/(P*) = ajp*]] - pfn, < 0, (3.4) 

representing a cone in the space of contact forces p^. Substituting (3.3) in 
(3.4), an expression of friction constraints in terms of external wrenches and 
internal forces (Jij(w,x) < 0 is obtained. In these terms, we can state the 
following 

Definition 3.2. A grasp is defined Force-Closure if, for any external wrench 
w aeting on the object, there exists a veetor x such that all friction constraints 
are fulfilled. 
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The analysis of force closure has been considered among others by [74], 
[28], [18], [73], while literature on the synthesis of force-closure grasps include 
[74], [79], [80], [81], [6j- _ _ 

According to the previous discussion on force closure, a crucial problem in 
robot manipulation is the choice of grasping forces so as to avoid (or minimise 
the risk of) slippage. The problem of choosing joint torques so as to realise 
the manipulating forces required by the task, while imposing internal forces 
that guarantee slippage avoidance, is often referred to as the force distribution 
problem. Further constraints on the choice of contact forces come from limi- 
tations in the object strength, or in the joint actuators torques. Accordingly, 
an “optimal” set of internal forces can be defined as the one that is fur- 
ther away from violating all such constraints. The force distribution problem 
is common with other robotic areas, as e.g. legged locomotion, cooperating 
and/or constrained manipulation, and has attracted much attention in the 
past few years (see e.g. [76], [46], [42], [54], [73], [106], [43], [78], [13]). 

An important property of the nonlinear constrained optimisation problem 
to which grasp force distribution amounts is convexity. This property, used 
Hrst in [6], enables efficient solutions to an otherwise very complex problem: 
[6] proposed numeric integration of an ODE as an iterative solution to the 
problem; [15] noticed that nonlinear friction constraints can be rewritten as 
positive-dehniteness constraints on suitable matrices, and used projected gra- 
dient flow methods to optimise; [52] further exploited the matrix formulation 
of [15] to transform the problem in the format of a standard linear matrix 
inequality (LMI) problem, for which off-the-shelf, effective software exists. 



4 Dynamics 

The ability to predict the dynamic behaviour of a grasp with a given model 
including the control algorithms, is critical to the design of the grasp. In 
multifingered grippers, as in legged locomotion systems, multi-arm systems, 
and other constrained robot systems, several limbs are used to constrain and 
manipulate an object [50], [54], [67]. The dynamic analysis and the simu- 
lation (the prediction of motion given the external forces and moments on 
the system) of such systems is central to the design of such systems and the 
development of control algorithms [107], [91]. 

A hand-object system is a constrained mechanical system, whose dynam- 
ical description can be derived using Euler-Lagrange’s equations along with 
constraint equations. The disjoint dynamics of the hand and of the object 
are written as 

M,i(q)q + Q,i(q,q) =r 

Mo(u)ii + Qo(u, li) = w, 

where M^(-) and Mo(-) are symmetric positive dehnite composite inertia 
matrices, and Q^(-, •) and Qo(-, •) are terms including velocity-dependent and 




64 



A. Bicchi, V. Kumar 



gravity forces of the hand and of the object, respectively. Hand and object 
dynamics are linked through the n rigid-body contact constraints (2.1). 

As we have seen before, when there are contacts between nominally rigid 
bodies, contact constraints are unilateral. Featherstone [27] , Lotstedt [57] and 
Mason and Wang [62] pointed out some of the inconsistencies which arise 
when rigid body models are used with Coulomb’s empirical law of friction in 
unilateral systems. For example, if we consider the simulation of a rod sliding 
along a rough ground in a plane with a single contact, there are configurations 
in which no solutions (that are consistent with the constraints) exist, and 
others in which the solution is not unique. Wang, Kumar, and Abel [108], 
[107] performed a dynamic analysis of the peg-in-the-hole insertion problem 
and showed that there was a range of parameters during two-point-contact 
for which there were either no solutions or two solutions for the accelerations. 
Quasi-static analysis is also known to exhibit such inconsistencies [36] . 

The inconsistencies and ambiguities in the dynamic analysis of frictional 
contacts have been attributed to the approximate nature of Coulomb’s model 
and to the incorrect assumption of rigidity. Recently, there has been some 
attention in the robotics community on overcoming these shortcomings by 
using rigid body models to predict the gross motion while using compliant 
contact models to predict the contact forces and the local deformations [49] . 

One of the main difficulties that is present in multifingered grasps, and a 
feature that is particularly true of such grasps as power grasps and enveloping 
grasps, is that the number of independent contact forces is much larger than 
the number of actuators. Thus, from a controllability standpoint, not all the 
contact forces are controllable (see Section 7 below). 

The analysis of statically indeterminate grasps or grasps in which there is 
no unique solution to the initial value problem is simply not possible unless 
one explicitly models the compliance at the contacts [20], [36], [74], [49]. Of 
course such contact models tend to be more complex and the parameters 
are more difficult to identify (see Section 6 below). Further, it is harder to 
simulate systems in which the time scale for the dynamics of contact inter- 
actions is significantly different from the time scale of rigid body dynamics 
[63], [98]. Thus, although efficient, approximate algorithms for “impulsive dy- 
namic simulation” that incorporate approximate impact models for collisions 
are available [66] , it is very difficult to write accurate simulators for dexterous 
and fine manipulation where the contact forces may be finite and the results 
may be sensitive to the parameters in the contact model. 



5 Stability 

A further important property of grasps is stability. The term is used in the 
literature with at least two meanings. One refers to Lyapunov theory, and 
dictates that a grasp is (asymptotically) stable if its dynamics are such that, 
when the object is displaced from its reference position, it will stay close (and 
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ultimately come back), to such position. A second definition is Lagrange’s, 
whereby a grasp in which all forces are conservative, is stable if it corre- 
sponds to a strict local minimum of the potential energy. The second usage 
is prevalent in studies on grasp stability. 

It is important to note that force closure does not guarantee stability. 
Any definition of stability must regard the grasp as a dynamic system and 
describe the properties of the dynamic system when it is perturbed from an 
equilibrium configuration. The role of compliance and dynamics in grasping 
has been investigated by many authors, beginning with Hanafusa and Asada 
[32] and Salisbury [61]. Cutkosky and Kao [21] discussed how to compute the 
aggregated compliance matrix of a hand-object system, including finger flex- 
ibility effects. Relations of compliant and rolling contacts with the stability of 
the grasp have been considered, at increasing levels of generality and detail, 
by [22], [70], [101], [36], [100], [29]. If Lagrange’s stability criterion applies to 
an equilibrium grasp for a conservative system, Lyapunov stability follows. 
It should be noted however that Lagrange’s analysis is limited under some 
regards. In mechanics, the seemingly intuitive statement that, if an equilib- 
rium point is not a minimum for the potential function, then it is unstable, 
does not have a proof for systems with more than 2 degrees of freedom [3]. 
Perhaps more importantly, from an application viewpoint, is the fact that 
no provision is made in Lagrange analysis for non-conservative forces (ex- 
cept for Rayleigh-type dissipative terms). Nonconservative forces may arise 
in grasping systems because of nonidealities in the mechanical components, 
and of the control laws used for actuating the hand joints. The inclusion of 
the effects of control on the stability of grasp, which are apparently of major 
moment, is as of today a mostly open research problem. Lyapunov stability, 
and other structural properties (controllability, observability, stabilisability) 
of general grasping systems in their linear approximation have been investi- 
gated by [12], [82], [2]. Stable control of manipulation and grasping systems 
has been considered among others by [72], [85], [93], [87]. Particularly im- 
portant is work done towards controlling grasping systems in the (practically 
ubiquitous) presence of uncertainties [17], [24]. 

A figure measuring stability (useful e.g. to compare different possible 
grasps) may be considered [36] as the real part of the dominant eigenvalue of 
the linearised grasp model (large values of this measure indicate that small 
perturbations are damped away quickly) . An even more useful figure, in many 
applications, would be related to the size of the basin of attraction of the equi- 
librium, indicating how large a perturbation can be without causing instabil- 
ity: however, effective algorithms to evaluate such measure are not available 
at present. 
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6 Contact Compliance 

The importance of modelling the finger-object contact and the role of compli- 
ance in grasping has been stressed by [4], [20], [95]. However, it is particularly 
difficult to model the relationship between small object/finger displacements 
and changes in contact forces arising from these displacements. 

Such contact problems have been studied extensively in the solid me- 
chanics community in the context of rail-wheel interaction [45] and analysis 
of ball and roller bearings [44]. There are difficulties even in establishing 
the uniqueness and existence of solutions of elastic bodies in static contact 
[25], and tractable analytical models are, in general, very difficult to come by. 
Hertz’s model [44] can be used to predict the pressure distribution across each 
contact patch when the contacts are frictionless and non-conformal. Hertzian 
contact theory is probably the most widely used analytical contact model, 
and variations of this are used in [36], [86]. 

Because friction is central to robotic grasp, the Hertzian contact model 
has proved to be inadequate in many cases. Sinha and Abel [95] proposed an 
elastic contact stress model for finger-object contacts in multifingered grasp- 
ing and a variational approach for quasi-static analysis. Wang, Kumar, and 
Abel [108] proposed a similar approach for dynamic analysis. They developed 
a mathematical programming approach for frictional, elastic contacts as well 
as viscoelastic contacts in which the inertial forces due to the deformations 
at the contacts are neglected. While such distributed parameter models yield 
accurate results, the solutions require computation-intensive numerical meth- 
ods. A possible simplification is provided by the Winkler elastic foundation 
model [44] , and the lumped parameter visco-elastic models used in [30] , [49] , 
[98] provide the simplest model for simulation and analysis. 

One of the very hard problems is getting an accurate and tractable model 
of contact compliance, particularly in the tangential direction. This is recog- 
nised to be a difficult problem in the mechanics literature as well [44]. In 
addition to this, a tractable and accurate model of friction, one that accu- 
rately predicts slip and one that lends itself to stability analysis, is currently 
not available. Both these fundamental problem areas are crucial to robotic 
grasping and contact analysis. 

For the purposes of analysis of grasp, it is generally assumed in the lit- 
erature that all contacts are point contacts and idealisations such as a line 
or surface contact can be approximated by two or more point contacts. Each 
point contact can be modelled as either a frictionless point contact, a fric- 
tional point contact, or a soft contact [88]. A frictionless contact is defined as 
a contact in which the finger (or effector/fixture) can only exert a force along 
the common normal at the point of contact. A frictional contact (sometimes 
referred to as a point contact with friction) is defined as a contact that can 
transmit both a normal force and a tangential force, while a soft contact also 
allows the finger to exert a pure torsional moment about the common normal 
at the point of contact. 
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7 Grasping and the Kinematics of the Hand 

It is interesting that much of the literature in grasping actually ignores the 
kinematics of the fingers or the articulations that are involved in contacting 
the object. While Reuleaux’s problem of form closure justifiably focused on 
the geometry of the object and the arrangement of contacts, it is difficult to 
analyse a grasp without modelling the dynamics, or at least the kinematics, 
of the fingers and the interaction of the fingers with the object. 

Trinkle et al explore the kinematics of enveloping grasps [102] using the 
restrictive but conservative assumption of frictionless contacts. The kinemat- 
ics of fingers with two or three point contacts with fingertips and palms have 
been studied by [77], [26]. While the analysis of form closure is intrinsically 
geometric, force closure is tightly linked to the kinematics of the end effector. 
In fact, it is possible that a geometric analysis of a grasp may predict force 
closure, but a careful analysis of the kinematics may reveal that this is not the 
case [35] . Definitions of force closure that take into account the kinematics of 
the gripping device were proposed in [6], along with an exact algorithm for 
testing such property. Yoshikawa proposes a new set of definitions for closure 
properties, including what he calls active and passive closures, to explicitly 
model the properties of the grasping mechanism [110]. Unfortunately, much 
of this and other related work [39] is based on instantaneous kinematics. 

Modelling of the fingers is particularly important when end effectors 
that have fewer degrees-of-freedom than necessary to impart arbitrary mo- 
tions/forces at all contacts. Such kinematically defective grasps are common 
in simple industrial grippers. If the hand Jacobian matrix is not full rank, it 
is not possible to command an arbitrary set of grasp forces [5] . This is usually 
the case in all power grasps. Modelling of the kinematics and manipulability 
of whole-hand manipulation in such systems is discussed in [83] . Intuitively, 
the more a grasp is defective, the more robust it is in restraining an object 
with respect to external distnrbances and the lower is sensitivity to posi- 
tioning errors, but also the lower is manipulability. However, a case-by-case 
analysis is necessary for optimal power grasps [6]. 

Many open problems remain to be solved so as to design robot hands to 
effectively exploit detectivity to increase grasp robustness and reduce hard- 
ware complexity. Among these, perhaps the most important is the need for a 
reliable estimate of contact compliance, arising with statically indeterminate 
grasps. This will allow the calculation of contact forces, and the development 
of models that relate joint displacements and torques to contact forces. 



8 Measures of Grasp Performance 

Recent work in the literature has tried to develop quality measures for grasps. 
One such measure can be derived from the conditioning of the grasp matrix 
and is directly connected with the closure properties of the grasp [54]. In a 
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similar fashion, other structural properties can be derived from the charac- 
teristic matrices, for example, controllability and observability [83] . 

When an object is restrained or grasped with multiple effectors, there are 
two, often conflicting, measures of grasp performance. First, if the fixtures 
can be accurately positioned, the system’s ability to reject wrench distur- 
bances is a measure of grasp stability. The grasp stiffness matrix, or a frame 
invariant measure of the minimum grasp stiffness [14] , provides one choice for 
a performance metric. This assumption of being able to accurately position 
the end effector is extensively used in the fixturing and grasping literature. 
However, when there are errors in positioning and orienting the end effectors, 
it is important to choose a grasp so that the system performance is insen- 
sitive to these positioning errors. Thus, it also makes sense to minimise the 
dependence of grasp forces on such positioning errors. 

Howard and Kumar [36] develop the theory needed to combine the stiff- 
ness matrices at each contact to calculate a grasp stiffness matrix. While 
the signs of the eigenvalues allow a test of grasp stability, the eigenvalues 
themselves are not invariant with respect to changes in reference frames [35] . 
Bruyninckx et al [14] develops a frame invariant measure of stability that 
is based on the grasp stiffness matrix and a metric on the Euclidean group. 
Lin develops a frame- invariant quality measure that essentially minimises the 
“object deflection” when the grasped object is subject to force disturbances 
[55]. The basic idea here is to scale the eigenvalues measuring the rotational 
stiffness by a characteristic distance to an edge of the object. Thus it is pos- 
sible to develop a scaled stiffness matrix and the smallest eigenvalue of the 
scaled matrix characterises the system. 

The focus in the above work is to quantify the ability of a fixture to reject 
disturbances due to external forces on the workpiece [23]. This is clearly a 
measure of performance that is relevant. However, the robustness of a grasp 
to errors in positioning the effectors has not been addressed in this literature. 
Sugar and Kumar develop a second measure of performance that characterises 
this robustness and discuss an approach to optimising fixtures based on both 
measures [99]. In this connection, the control of grasping and the effects of 
of uncertainties are particularly important. 

Unfortunately most of these measures are based on the assumptions of 
small perturbations: displacements, forces and errors. It is no question that 
global measures would be more useful. For example, a figure relating to the 
size of the basin of attraction of the equilibrium, indicating how large a per- 
turbation can be without causing instability would be desirable. However, the 
nonsmooth nature of grasp dynamics considerably complicates the analysis. 



9 Concluding Remarks 

This chapter has presented a survey of work in robotic grasping and ma- 
nipulation over the last twenty years. It is impossible to do justice to all 
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the work in this area, particularly because of the breadth of the field and 
its close connection to dexterous manipulation, fixturing, and haptics. We 
chose to focus on issues that are central to the mechanics of grasping and the 
finger-object contact interactions. In addition, the review mainly addressed 
research that has established the theoretical framework for grasp analysis, 
simulation and synthesis. Because of the limitations on space, we have not 
given the algorithmic aspects, and the applications the attention that they 
deserve. 
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In several fields of robotics, tactile and force sensors represent a basic tool 
for achieving an enhanced interaction with the environment. As a matter of 
fact, areas such as advanced manipulation, telemanipulation, haptic devices, 
legged robots and so on are intrinsically based on an advanced sensorial 
equipment and on proper techniques for the exploitation of their information. 
These types of sensors give information such as the presence of a contact, its 
size and shape, the exchanged forces/torques. More advanced sensors can also 
provide additional information, such as mechanical properties of the bodies 
in contact (e.g. friction coefficient, roughness, . . . ) or the slippage. In this 
chapter, an overview on tactile and force sensors and their specific use in 
robotic manipulation is presented. In particular, after an illustration of the 
state of the art and of the main recent technological developments, results 
concerning the detection and control of the relative motion (slippage) of two 
bodies in contact are discussed. This problem may be of relevance, e.g. in 
advanced manipulation by robotic systems in which, depending on the task 
to be executed, it might be desirable either to avoid or to exploit the slippage 
of the manipulated object. Similar problems can be found with legged robots, 
or more in general in any case where a robotic device has to interact with 
its environment in a controlled manner. These and other applications have 
justified an increasing research effort in this field in the last years, generating 
several interesting prototypes and techniques for data analysis. 



1 Introduction 

In the last decades, the development of new technologies has allowed a growth 
of applications for industrial robots, which in turn has led to important 
achievements in the production field as well as in other sectors, such as med- 
ical care, support for disable people, help to human beings in hazardous 
environments. In this wide scenario, one of the main bottlenecks which still 
limits an even broader diffusion of robotic manipulation systems is the sen- 
sorial equipment of these devices. As a matter of fact, a typical industrial 
robot is equipped only with a relatively poor sensory capability, consisting 
of joint position sensors. Then, depending on the applications the robot may 
be equipped with additional sensorial devices, such as force/torque sensors, 
video cameras, or special sensors designed for the specific task, for example 
sensors for measuring temperature, pressure, humidity and so on. 

S. Nicosia et al. (Eds.): RAMSETE, LNCIS 270, pp. 75-102, 2001. 

© Springer-Verlag Berlin Heidelberg 2001 




76 



C. Melchiorri 



The limited sensoriality of a robot manipulator has two main implications. 
On one hand several tasks are not achievable by current robots since they are 
not able to handle complex situations due to their limited knowledge of the 
environment. On the other hand, even when tasks are possible, the program- 
ming efforts (and therefore the time needed for the set-up phase) is much 
larger than in case of a sensorised manipulator. Another important consider- 
ation derives from the fact that in the last years new kinds of manipulation 
devices have been, and still are, developed both in the research and in the 
advanced application domain (space, underwater, medical, . . . ) . These new 
devices can be generically considered as ‘advanced manipulation systems’, 
and may consist of multi-fingered robotic hands, cooperating robots, walking 
machines, and so on [34], [40], [9]. These manipulation devices requires as a 
basic need an advanced sensoriality, able to measure and monitor the inter- 
actions with manipulated objects, and more in general with the environment. 

In robotic manipulation, two main families of sensors for measuring the 
variables of interest during interactions with the environment can be men- 
tioned. The first category consists of sensors measuring properties of the 
environment without a direct interaction with it: vision systems, US-based 
sensors, proximity sensors, etc.. The second category includes sensors which 
provide a measurement of variables generated during the physical interaction 
between the robot and the environment. Typical examples are force and/or 
tactile sensors. 

Undoubtedly, more attention has been devoted by the robotic and artifi- 
cial intelligence communities to the first class of sensors. Reasons of this fact 
probably are the commercial availability of economic vision or US systems, 
and the fact that often an analysis of static information of the environment 
is sufficient for planning the tasks of the robotic device. 

On the other hand, it has been recognised the importance of the avail- 
ability of more precise data during the evolution of a manipulation task. 
Information such as friction, applied force, incipient slippage, thermal prop- 
erties, and so on, cannot be obtained using a vision or US sensor. Moreover 
there may be factors, for example a poor illumination of the environment, 
which limit or do not permit at all the use of this type of sensorial devices. 
For these reasons, in the last decade a noticeable activity has been devoted 
to the development of tactile and force sensing devices. 

In this chapter, problems relative to tactile/force sensing in robotic ma- 
nipulation are considered. An overview on the state of the art and on the 
technologies adopted for the realisation of tactile and force sensors is given 
in Section 2. In Section 3 the use of force sensors as tactile (Intrinsic Tac- 
tile) devices is illustrated, as well as a tactile matrix sensor and its use for 
manipulation purposes. In Section 4, techniques for slip detection and con- 
trol are described, reporting also an overview on recent results in this field. 
Results obtained for integrating the two sensors are illustrated in Section 5, 
for both detecting the slip and controlling it in order to execute some type 
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of manipulation. A key point here is that the integration of the two sens- 
ing devices allows obtaining new capabilities, not achievable with the single 
sensors alone. The conclusive section reports comments and future research 
directions. 



2 An Overview on Force/Torqne and Tactile Sensors 

As previously mentioned, force and/or tactile sensing is one of the most 
basic requirements for any manipulator interacting physically with the envi- 
ronment in a non-conventional or non-structured manner. By means of direct 
measurements on the contacted objects, it is possible to apply control and 
behaviour strategies in order to accomplish complex tasks in autonomous or 
semi-autonomous way. Other sensory equipment can provide only generic in- 
formation on manipulator’s interaction with the environment. For example 
vision systems, that cannot give precise and local information on the contact 
state, are often used in the planning phase. For these reasons, the design of 
tactile/force sensors and the development of techniques to exploit their in- 
formation are considered relevant research areas for the robotic community 
[32], [31], [26], [17], [25]. 

2.1 Force Sensors, IT Sensing 

Commercially available force sensors are devices to be installed mostly at 
the wrist of manipulators, and are manufactured by several industries in the 
United States, Japan and Europe. Examples of use of these sensors, still quite 
limited in number if compared with their potentialities, are found in advanced 
manipulation and assembly. 

Basically, the major part of these devices are transducers which measure 
forces/ torques by means of the induced mechanical strains on flexible parts 
of their mechanical structure. The mechanical strains are in turns measured 
by elastomers (strain gauges), properly glued on the structure, that change 
their resistance according to local deformations, see a schematic drawing in 
Figure 2.1. Other technologies, like the optical one, have been used to realise 
force sensors [19], but have received less attention than the one based on 
elastomers. 

For sensors based on the strain gauge technology, a linear mapping be- 
tween the readings of the strain gauges, collected in a vector s G IR", and 
the applied wrench w = [f^, m”^]^ G H® is commonly assumed. This linear 
mapping is expressed by a matrix, called the calibration matrix C G 

w = C s. (2-1) 

The basic capability of a force sensor is to measure the force/torque vector w 
applied during the interaction with the object/environment. A major advance 
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Figure 2.1. Conceptual scheme of a force/torque sensor (left) and a sensor con- 
nected to a link of known shape (right) 



in force sensing has resulted after a paper of Salisbury in 1984 [33], where 
he proposed to use a force sensor for obtaining, besides the wrench w, other 
significant information about the contact state. In particular, the position 
of the contact centroid [8] may be easily computed under some hypotheses. 
Later, Bicchi and Dario have extended this concept introducing the term 
Intrinsic Tactile (IT) sensing [7], [4]. 

The basic idea of the IT principle is illustrated in Figure 2.1. Given a 
force/torque sensor with known external shape and connected to a link of a 
manipulator, the basic feature of IT sensing is the possibility to determine, in 
case a contact is established between the link and an object, both the applied 
wrench w and the position p of the contact centroid on the surface of the 
link. 

Successively, Eberman and Salisbury have extended the possibilities given 
by force sensors, considering not only “static” information about the wrench 
and the contact point location, but also the “dynamic” data obtained with 
a frequency analysis in different working conditions, [15]. A different idea 
concerning the dynamic use of force sensor has been more recently presented 
by Bicchi et al in [6]. Here, a force/torque sensor is used for measuring also 
impulsive forces and, since the sensor is de facto built with a flexible structure, 
the inversion problem is faced as an inversion dynamic problem typical of 
nonminimum phase systems. 

One of the problems of the IT principle is the need of installing force 
sensors in the mechanical structure of the robot. While there are no relevant 
problems with “large” structures, the applicability of these sensors is quite 
limited for small devices, for example dexterous hands, due to technological 
difficulties of developing sensors of suitable size, both from the mechanical 
and the electronic point of view. 

Only a few miniaturised 6-axis force sensors have been developed. Proba- 
bly, the hrst was the one realised in 1985 at MIT [11], with a design based on 
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Figure 2.2. The U.B. Hand version II (left) and a three degrees of freedom gripper 
for space applications (right) 



a Maltese cross and employing 16 strain gauges. This sensor has been used 
in the Salisbury Hand. 

A second prototype has been realised in Pisa [7], with 6 strain gauges 
placed on a hollow cylinder. Note that 6 is the minimum number of strain 
gauges required to measure the 6 elements of the force/torque vector. In this 
sensor, the placement of the sensing elements has been optimised with respect 
to the condition number of the calibration matrix C relating the strain gauges 
output and the applied forces [3]. A third miniaturised 6- axis force/torque 
sensor has been realised in the framework of the UB Hand project [9], see Fig- 
ure 2.2. Due to very stringent mechanical constraints, this sensor has a very 
compact design, both in the mechanics and in the electronics, and employs 6 
or 8 strain gauges glued on the sides of a square bean [12], see Figure 3.1. A 
description of this sensor is given with more details in Section 3.1. Another 
and more recent application of this type of sensor is in the framework of a 
3-finger, 3-DOF gripper for space applications (Figure 2.2) where one sensor 
is present in each of the fingers to ensure robust grasp of unknown objects 
and control of the applied force, see [2] . After the initial developments of pro- 
totypes in research centers, nowadays some miniaturised force/torque sensors 
are also commercially available devices, see e.g. [1]. 

2.2 Tactile Sensors 

Another very important class of sensing devices consists of tactile sensors. 
These are used for several purposes, such as shape recognition, contact point 
determination, pressure/force measurement. A number of tactile sensors have 
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been proposed in the literature, with several different solutions concerning 
the realisation features: optical, piezoresistive, piezoelectric, and so on. See 
the surveys given in [32], [31] and more recently in [26] for good overviews 
on technologies and applications. 

Tactile sensors have been introduced in robotics since the late 70’s. Nowa- 
days, as the force sensors, also tactile sensors are commercially available de- 
vices. Probably, they represent the most commonly adopted sensorial class 
for industrial grippers, although often they are used as advanced ‘on/off’ 
devices to check whether a grasp or contact condition occurs. 

Usually, they consist in a matrix (array) of sensing elements. Each sensing 
element is referred to as a taxel (from “tactile element”), and the whole set 
of information is called a tactile image. Main goal of this class of sensors is 
to measure the map of pressures over the sensing area. 

The types of information that may be obtained from a tactile sensor are: 

— contaet: this is the most simple information given by the sensor, concerning 
the presence or absence of a contact; 

— foree: each sensing element provides an information related to the amount 
of locally applied force, which can be used in several manners for successive 
elaborations; 

— simple geometrieal information, i.e. position of the contact area, geomet- 
rical shape of the contact itself (planar, circular, and so on); 

— main geometrical features of the objeet: by proper elaborations of the data 
of the taxels, it is possible to deduce the type of object in contact with 
the sensor, for example a sphere, a cylinder, etc. (data relative to the 3D 
shape) ; 

— mechanical properties, such as friction coefficient, roughness, and so on: 
also thermal properties of the object may be measured by a tactile sensor; 

— slip condition, i.e the relative movement between the object and the sensor. 

2.3 Technology for Tactile Sensing 

Since the introduction of tactile sensors in robotics in the 70’s, several tech- 
nologies have been adopted for their design, ranging from piezoresistive to 
magnetic and optical effects, [31, 26]. In the following, the main technological 
solutions are briefly described, illustrating some of the main advantages and 
disadvantages of each of them. 

2.3.1 Resistive and conductive effect. Probably, this is the first and 
most popular method for transducing forces or pressures into electric sig- 
nal, [36], [10]. Basically, this technique is based on the measurement of the 
electrical resistance of a material (elastomer) whose value depends on the 
locally applied force. Most common materials used as elastomers are made 
from carbon or silicon doped rubber. A grid of sensing points is built, where 
the resistance is measured, either across or through the elastomer, see Fig- 
ure 2.3. These sensors are usually very robust, with respect to both durability 
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Figure 2.3. Tactile sensors based on resistive effect. R: rubber, E: electrodes 



and mechanical effects, have a good dynamic range, and can be quite easily 
integrated with the acquisition electronics. On the other hand, the function 
relating the force and the resistance variation is not linear and may present 
some hysteresis effects. Moreover, a relevant number of wires is necessary 
(this is a problem common to all the matrix sensors). 

2.3.2 Electromagnetic effect. Electromagnetic effects have been widely 
used in tactile sensing. Variation of a magnetic field, induced either by a 
mechanical movement of a part of the sensor or by an intrinsic property 
of a magneto-elastic material, can be easily detected and measured. Usually, 
these sensors have good performance, both in terms of linearity and hysteresis 
effects, may have the possibility of measuring the normal, tangential and 
torque component of the applied load, and are intrinsically robust. They are 
sensitive to extraneous electromagnetic fields. 

2.3.3 Capacitive effect. The capacitance of a capacitor may change by 
varying the distance or more in general the coupling between its elements. 
This effect has been exploited in several manners for building tactile sensors, 
see for example Figure 2.4. Usually, these sensors have a linear response and 
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Figure 2.4. Tactile sensors based on capacitive effect 
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have an intrinsically robust design. On the other hand, there may be electrical 
disturbances, sensitivity to temperature, and dimensioning problems. 

2.3.4 Piezoelectric effect. The piezoelectric effect of some materials, i.e. 
the generation of a voltage under the application of a pressure, can be easily 
exploited for tactile sensing. The generated voltage is proportionally related 
to the amount of pressure, and no external voltage is necessary. A similar 
effect, which has been used in some cases, is the pyroelectric effect, i.e. the 
generation of voltage with the change of temperature. 

A possible drawback is that both these effects are dynamic, i.e. the voltage 
is generated during the application of a pressure or a change of temperature. 
In static condition, the effect is null. Another disadvantage is that often 
piezoelectric materials are basically crystals, such as quartz or ceramics: these 
materials are rather fragile and not suitable for this kind of applications. On 
the other hand, also sensors based on polymeric materials such as PVF2 
or PVDF have been successfully developed. Technological difficulties derive 
from the fact that each taxel need to be acquired by an individual electronics 
(i.e. the charge of each taxel must be acquired individually). 

2.3.5 Optical effect. Tactile sensors based on optical effects exploit the 
modulation of light beams obtained by the deformation of transmitting ma- 
terial and/or reflection due to the object itself. Advantages of these sensors 
are the robustness with respect to external electrical disturbances or interfer- 
ence, the fact that the acquisition and elaboration electronics can be remotely 
located with respect to the sensor, the good compatibility with vision sensing 
technology. Drawbacks are mainly related to technological problems, such as 
the relevant number of fibers necessary for obtaining a good resolution. 

2.3.6 Mechanical methods. A mechanical displacement originated by the 
contact is used as basic principle of some tactile sensors. Usually, these sen- 
sors are used as on/off devices, or as probe systems for measuring or weld- 
ing apparatus. In order to measure the displacement, a number of different 
techniques have been employed, ranging from potentiometers to acoustic or 
electrical effects. This type of sensor usually is rather robust and reliable, but 
on the other hand gives some problems for building sensing arrays, and in 
any case for the relatively small achievable dimensions. 



3 Examples of Force/Torque and Tactile Sensors 

In the following, two prototypes of sensors for advanced robotic manipulations 
are briefly described. The first is a miniaturised force/torque sensor, suitable 
for installation in fingers of robotic hands, while the second is a tactile matrix 
sensor based on a conductive rubber. The sensors are described and examples 
of their use in advanced manipulation tasks are presented. 
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3.1 A Miniaturised Intrinsic Tactile Sensor 

As already pointed out, the need of a force sensoriality is often a basic require- 
ment for advanced robots. Moreover, in several applications the dimensional 
constraints of both the mechanical and the electronic part call for the devel- 
opment of sensors which can be easily installed and used in robots of small 
sizes. 

Some of these devices have been realised at the University of Bologna, 
characterised by small dimensions and a good versatility. The first has been 
developed in the framework of the UB Hand project [9], where the need of 
a distributed force sensoriality led to the design and development of minia- 
turised 6-axis force sensors. More recently, this type of sensor has been also 
adopted in other prototypes of robotic grippers, such as 3-DOF grippers de- 
veloped for space applications, [2]. 

The compactness of both the mechanical structure and the electronics, 
as well as the accuracy, resolution, and simplicity of the acquisition process 
have been main design and realisation objectives for these sensors [12]. The 
sensing element is realised by a light compliant mechanical structure with a 
parallelepiped shape, see Figure 3.1. a and Figure 3.1.b. In the first design, 
the sensor height, width and depth are respectively 21, 24, and 15 mm. 

The base of the sensor is fixed to the link structure, while the other 
extremity is rigidly connected to the external shell. Contact wrenches produce 
a deformation of the sensing element proportional to the amount of force, and 
the deformation is measured by strain-gauges. As an example of installation, 
in Figure 3.1.C the arrangement of the sensors within the phalanges of one 
finger of the UB Hand is reported, see also Figure 2.2, while in Figure 3.1.d 
its application in the three-fingered gripper is shown. 

In order to reduce at minimum the complexity of the sensor, the strain 
gauges are not used in a bridge configuration, and are serially connected. The 
compensation of the thermal effects on the resistance value is performed by 
the conditioning electronics, by means of a comparison of the voltage values 
given by the deformed strain gauges with the value of an undeformed one. A 
voltage reference is used as power supply. 

These sensors have been already described in details, see e.g. [12], as 
well as their applications to robotic manipulation. In particular, with this 
sensor it is possible to: determine the position of the contact centroid on the 
finger surface; estimate the linear and rotational friction coefficient; control 
the applied forces/torques; verify whether the grasped object is close to a 
slipping condition. See [25], [33], [8], [12], [2], [30] for a description of these 
and other applications. Some results concerning the determination of the 
contact centroid are reported in Figure 3.2 with shells of different shapes. 

In Figure 3.3 results for the measurement of the friction coefficient are 
shown: here the friction coefficient is computed as the ratio between the 
tangential and normal force components ft, fn- 
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Figure 3.1. Two IT sensors (a) and (b), their placement within fingers of an 
anthropomorphic hand (c) and of a gripper (d) 



Ms = 7^- (3.1) 

Jn 

A similar approach can be used to determine the rotational friction coef- 
ficient, although in this case the coefficient is not independent on the size of 
the contact area, see [30] and [5] for more details. 

Obviously, if the friction coefficient is known, this information can be used 
in order to apply proper control action if a slippage condition occurs. 




Figure 3.2. Measurement of the contact centroid and of the applied forces with 
links of different shape 



3.2 A Tactile Matrix Sensor 

An example of tactile sensing, used in the activities presented in the follow- 
ing, is the matrix tactile sensor developed at Delft University of Technology, 
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Figure 3.3. Determination of the linear friction coefficient: normal and tangential 
components (left) and their ratio (right) 



[38]. As previously mentioned, a tactile matrix sensor measures the forces 
applied on it and how these forces are distributed over the sensor surface. 
Such a sensor in a robot system is able to provide information similar to that 
provided by human tactile sensors in the skin. 

This tactile matrix sensor is based on conductive rubber. Note that since 
the adopted rubber is rather soft, it also enables a good grip on objects. 
The resistance of the rubber depends on the number of conductive particles 
inside the rubber (i.e. on the type of the rubber), on its geometry, and on 
the applied force. 

The response of the rubber, as a result of the applied force, is highly 
non linear. Therefore, it is not possible to obtain an accurate force sensor 
with this technology, see Figure 3.4 for a typical characteristic of the rubber 
resistance as a function of the applied force. However, the rubber is capable 
of detecting the force distribution over the sensor area. 




Figure 3.4. Resistance of the rubber as a function of the applied force / 



In order to locally measure the resistance of the rubber, and therefore the 
value of the applied force, two electrodes are needed, since the resistance is 
measured by applying a current between the electrodes through the rubber. 
A pair of electrodes will be referenced to as a taxel. 
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The overall sensor consists in a matrix of 16 x 16 taxels, with a side length 
of about 2 cm, and a distance between two consecutive taxels of 1.25 mm. A 
printed circuit board has been used for the realisation of the electrodes, with 
a matrix (row/column) structure, see Figure 3.5. 
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Figure 3.5. Tactile matrix sensor surface (4x4) with partial cross-section 



The tactile sensor is able to detect 256 pressure levels and has a variable 
gain whereby the sensor sensitivity is automatically adjusted to ensure that 
maximum use is made of the available resolution. Some examples of images 
obtained by the tactile matrix sensor are given in Figure 3.6. Each grid point 
represents a taxel value between 0 and 255. 




Figure 3.6. Some images of the tactile sensor 



This sensor has been successfully used for typical applications, as ob- 
ject recognition. Different techniques have been implemented either based on 
neural network, see [14], [20], or on more conventional techniques based on 
successive data elaborations [27]. Moreover, more advanced applications have 
been developed, as also described in the following section. 
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4 Slip Detection 

In the field of fine manipulation, consistent efforts have been devoted to de- 
signing sensors capable to detect a slip condition or even an incipient slip 
condition. The development of such sensors is very useful for example in 
the estimation of the friction coefficient of an unknown object during ma- 
nipulation or also in exploiting the slip condition to improve manipulation. 
Accurate knowledge of the friction coefficient is particularly important for 
fine manipulation and surface tracking. In other words, e.g. when performing 
fine manipulation with fragile objects, it is important that the grasp force 
is maintained just above the minimum required to avoid the slippage of the 
object. During surface tracking, accurate and updated knowledge of friction 
conditions is important to avoid losing track of the object. Different method- 
ologies have been adopted to achieve the above goal. In particular, one can 
distinguish two main approaches. 

The first consists in calculating with a force sensor the friction coefficient 
by measuring the normal and the tangential forces when the object starts 
to slide. The main advantage of this approach is that it can be implemented 
without using dedicated sensors, although the measure is obtained in conse- 
quence of a relative movement of the object and the grasping device. 

The second approach consists in the development of special sensors. These 
are mainly dedicated devices [13], [22], [37], designed and realised with the 
only goal of detecting a slip condition. Mostly, they are based on accelerom- 
eters placed in elastic materials which starts vibrating when slip begins. The 
main advantage of these sensors is that they provide a robust and a reliable 
measurement. On the other hand, they are dedicated devices and therefore 
increase the overall complexity of the sensorial system. 

A different technique consists in exploiting the data provided by a tactile 
matrix sensor. Note that by using this type of sensor, capable of measuring 
also other significant features, such as the shape of the grasped object, con- 
tact points and a pressure-force distribution, the complexity of the overall 
sensorial system is limited. 

4.1 Slip Detection with a Tactile Sensor 

A method for slippage detection using a tactile matrix sensor and based 
on the rubber elasticity has been developed and experimentally tested, see 
[28], [21]. As a matter of fact, before the object starts to slip the rubber is 
stretched: this deformation can be interpreted as a signal of an incipient slip. 
In fact, a slip can be detected by implementing a frequency analysis of the 
center of the force distribution and by testing the low frequency components. 

This approach employs the elasticity of the rubber. In particular, when 
a tangential force is applied to the object, the local shape of the rubber is 
modified. One consequence is that the center of distribution changes its posi- 
tion. By implementing a Fourier analysis and by testing the low frequencies, 
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it is possible to recognise the deformation that precedes the real slip. It is 
important to note that this effect is independent on the size and type of the 
object. 

In detail, the algorithm involves the following steps. A new position of 
the center of distribution is calculated every sampling period as: 



^ YTi=l 



„ ^ ELl 
E” 1 

— \ r I 



n = 256 



where (j)i is the pressure value of the i-th taxel (proportional to the local 
rubber resistance), and Xi,yi are its coordinates on the tactile matrix. The 
Cartesian coordinates of the center of distribution are then transformed in 
polar coordinates (p, 6) and, for the sake of simplicity, only the component 
P = V^c ~^Vc i® considered. Then, a FFT of p is computed as: 



N-l 

i?(fc) = ^ k = l,2,...,N. 

i=0 

When no slip is present, and disregarding the component at zero frequency, 
the spectrum does not give any significant information since it is dominated 
by noise, see Figure 4.1. a. 




Figure 4.1. Typical spectrum without (a) and with (b) slip 



On the other hand, when a tangential force is applied and the rubber 
initiates to deform, just before the object starts to slip the low frequency 
components dominate the spectrum, which assumes a well definite shape, 
see Figure 4.1.b. This effect is due to the shift of the center of distribution 
at low speed. It is important to note that this effect is present before the 
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object actually starts to slip (i.e. when the object is still stuck to the rubber) 
and also if the object completely covers the sensor. Once the object starts to 
slip, the center of gravity is stable again and the DC component dominates, 
generating a spectrum as in Figure 4.1. a. This means that this technique is 
capable only to measure the initial phase of slip since it gives, in practice, 
the same results when the object is fixed and when it moves with a constant 
velocity due to the application of a constant force. 

In Figure 4.2 results of a typical experiment, obtained with the setup 
described in Section 5, are reported. In particular, in Figure 4. 2. a the move- 
ment of a grasped object is shown, as measured with a precise position sensor, 
along with the first 3 components of the FFT. It may be easily seen that the 
information given by the spectrum allows detecting the incipient slip of the 
object. Similarly, also a pure rotational slippage of a grasped object may be 
detected, see Figure 4.2.b, although in this case the detection of the rotation 
is most likely due to nonlinearities of the rubber more than by a motion of 
the center of distributions. 






^ 




' 2 First FFT (Amp. <1 Hz) 4 5 6 








^ 2 Second FFT%omp. (2 Hz) 4 5 6 








' 2 Third FFT Amp. (3 Hz) *• 6 6 










Figure 4.2. Linear (a) and rotational (b) slip detection 



The experiments demonstrate that this technique is very efficient in de- 
tecting the initial phase of the slip, while on the other hand gives poor results 
during the motion of the object. 

4.2 Slip Detection with a Tactile and Force/Torque Sensor 

By using a tactile sensor alone, as discussed in the previous section, it is possi- 
ble to detect the linear slip between an object and the grasping manipulator. 
In a more general context, it may be of interest to detect also the rotational 
slip which can occur during manipulation. In this respect, a force/torque or 
a tactile sensor alone are not adequate, since on one hand the ‘rotational’ 
friction coefficient in general is not expressed by a simple linear relationship 
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as (3.1) and on the other the knowledge of the applied wrench is of interest. 
In the following, an activity for integrating the two sensors is summarised. 
For more details refer to [30], [5], [29]. 

In case of rotation, in general a nonlinear relationship exists between the 
applied normal force /„ and the torque nif [24]. This relationships may be 
expressed as 

ruf = (4.1) 

where a is a parameter depending on the geometry and on the elastic proper- 
ties of the contacting bodies. Equation (4.1) may be approximated, in suitable 
ranges of torques m/, by 

rrif = Psfn- (4.2) 

The friction coefficient Ps (or /3() depends on the size of the contact area, and 
this geometrical information cannot be provided by the force/torque sensor, 
although it can provide, for a given contact, information about the ratio 
mf/fn- 

In the literature, different models have been defined for describing the ef- 
fects of friction with linear and torsional loads in the contact area, see e.g. [16], 
[23], [18]. For practical applications, important issues are the simplicity and 
the adequacy of these models, that must be evaluated in real time. Among 
others, the models illustrated in [23] represent quite simple tests for real-time 
implementation. In particular, the linear model 



A 

Us 




(4.3) 



and the elliptical one 




(4.4) 



are both simple and sufficiently adequate for describing the phenomena of 
interest, and give very simple conditions for checking a slip condition. On 
the other hand, implicit drawbacks of these models are the fact that they are 
based on global information (i.e. the overall force and the torque) and that it 
is not simple to have a good estimation of the rotational friction coefficient 
Ps, since it depends on the size of the contact area (non measurable with 
force sensors alone). 



4.3 Rotational and Translational Slip Detection 

When only linear forces are applied at the contact, if a Coulomb friction 
model is considered with static coefficient ^s, from (3.1) a simple condition 
to check whether a relative motion between two bodies takes place consists 
in verifying the inequality 

(\/ fh + -^/y) //" - 



ff/fn < 



(4.5) 
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This simple method has been successfully used in several experiments for slip 
detection and control using a IT sensor, see e.g. [25] and [4], 

On the other hand, this condition is not sufficient in case the forces acting 
on the object generate a torque component on it. As already pointed out, in 
this case a more general relationship must be defined, taking into account 
also the geometry of the contact area. For these reasons, we consider in the 
following the integrated use of a IT and a tactile matrix sensor, see also [30], 
[5], [29]. 

In general, the motion of a rigid body (in contact with a surface) is a 
rotation about a point called Center of Rotation (COR). A particular case is 
when this point is at an inhnite distance, and the motion is a pure translation. 
When the body starts to slide, the directions of all the tangential forces (j)f 
on the contact surface cause the body to rotate about the COR. This means 
that at the limit condition these friction force components (applied at the 
generic point Pi) have direction perpendicular to the line connecting to 
the COR, i.e. (pi — Pc) = 0, see Figure 4.3. 




Figure 4.3. The direction of the local friction force <pf at the limit condition is 
perpendicular to pi — pc 



A practical approach to test the slip condition is reported in the follow- 
ing. Let us suppose that the COR position p^ = [xc j/c]^ is known (with 
respect to a given reference frame if). Moreover, we consider to use both a 
force/torque sensor (by means of which we obtain the normal and tangential 
force fn and ft, and the overall torque mt) and tactile matrix sensor (by 
means of which we measure the size and shape of the contact area A and 
the distribution of local pressure). From the combination of the two sensors, 
it is possible to evaluate also the local normal forces </>„ in each taxel of the 
tactile sensor. 

Considering the ‘discrete’ nature of the tactile sensor, and assuming to 
be at the limit condition, it is possible to compute the torque nic (about z) 
given by the friction forces acting on A with respect to the COR as: 
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me 



256 



X (p* -Pc)| 

256 

^ ^ \/ “1“ {yi 



i=l 




(4.6) 



Then, it is possible to compare me with the torque m* measured by the force 
sensor. No (rotational) motion takes place between the two bodies if 

256 

mt < me = - Xe)^ + ivi ~ VcY- 

i=l 



Since also an external force ft can be applied to the object, by defining 
nt = I mt + /t X Pel, the condition to check whether slippage takes place 
becomes 

256 

fit <me= ~ VcY ■ ( 4 - 7 ) 

i=l 

From a theoretical point of view, in this manner it is possible to verify whether 
a slippage takes place, verifying at the same time (with the COR position) if 
it consists of a rotation and a translation or of a pure translation. 

From the above, since the quantities rrit, ft, <j)n,i are known from the two 
sensors, it follows that the problem reduces to the determination of the COR 
position Pc, from which, by means of (4.6), it is possible to compute me- 
in practice, once the external tangential force ft = [ftx fty acting on 
the body is known, as well as the distribution of the local force components 
(pf and (p-ei and the geometry of the contact area A, the COR position Pc can 
be computed from the following equations, see also Figure 4.3, 



< 



ftx 


256 

-E 

2 = 1 
256 


II 


E 

2=1 



^spn,ifyi Vc) 

{Xi - Xe)^ + (Ut - VeY 
^c) 

a / {Xi - Xe)^ + (Ut - VeY 



(4.8) 



being Xi,yi the position of the i-th taxel. Equations (4.8) allows computing 
the unknown COR position, i.e. Xe,Ue- Note that we have assumed in (4.8) 
a constant friction coefficient in each point of the contact surface. Moreover, 
note that these equations are valid only at the limit condition for slippage, i.e. 
when \(j)f\ — y,s4>n- On the other hand, the computation of the COR position 
makes sense only after the slip motion begins, being null the relative motion 
of the objects beforehand. Finally, the possibility that the COR coincides 
with one of the taxels of the tactile sensor must be explicitly considered, 
since in this case (4.8) cannot be used. 

In order to solve the nonlinear equations in (4.8) in the unknowns Xe,yc, 
a recursive technique has been adopted, similar to methods for the solution 
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of the inverse kinematic problem of robot manipulators, [35]. 

The relationship in (4.7) represents an extension of (4.5) to the case of 
both forces and torques applied at the contact, and is more general than 
(4.3)-(4.4) since it takes into consideration the size of the contact area and 
avoids the computation of the rotational friction coefficient fig- 



5 Experimental Results 

The techniques described in the previous sections have been experimentally 
verified by using laboratory setups integrating the force/torques and the tac- 
tile sensors. In particular, the first setup consists in a planar 2-DOF ‘finger’, 
with a ‘hngertip’ equipped with the IT and the tactile sensors, a hxed sur- 
face for holding an object, and an external motor, with a precision position 
sensor, for applying forces/torques to the object and measuring its displace- 
ment, while the second is a two 1-DOF fingers gripper, again equipped with 
the two sensors, see Figure 5.1. 



External 





Figure 5.1. The two laboratory experimental setups 



The results reported in the following refer to the detection and con- 
trol of linear slippage, based on the IT sensor alone, and to the rota- 
tional/translational slip detection, involving the use of the IT sensor alone 
and the integration of the IT and the tactile matrix sensor. 
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5.1 Translational Slip Detection and Control 



If the friction coefficient is known, e.g. by means of a measurement as in 
Figure 3.3, it is possible to use it in manipulation strategies in order, for 
example, to prevent slippage of the object. 

The simplest control strategy for avoiding slippage is to use (4.5) to com- 
pute in real time the normal force /„ needed to balance the applied tangential 
force ft ■ A more convenient solution consists in adopting a conservative scal- 
ing factor cr < 1 for the friction coefficient. Moreover, it may be necessary 
to bound the computed value of /„ in order to neglect measurement noise 
and to avoid the application of high forces. Therefore, the normal force /„ is 
computed as 



I /"I - J/i, > 

\ fn2 — aiax{/^l, /nom} 



fn — Olin'[yVi2 5 fraax\ 



where fnom is the nominal force to be applied when no tangential components 
are present, and fmax is the maximum applicable value. Results obtained with 




Figure 5.2. Linear slip avoidance with a conservative values of (<t = 0.5) 



this technique are shown in Figure 5.2. In the figure, the value a — 0.5 has 
been used. Each dot represents the applied normal force as a function of the 
tangential force, the diagonal solid line represents the border of the friction 
cone: the slip is always avoided. In the experiment, the measured friction 
coefficient was /tg = 0.45. A similar strategy to prevent slippage has been 
proposed in [4]. 

5.2 Rotational and Translational Slip Detection and Control 

Although even the rotational slippage may be detected by using a force/torque 
sensor alone, this possibility is intrinsically limited by the fact that the di- 
mension of the contact should be known in advance, and moreover it should 
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be constant during manipulation. In the following, an integrated use of a 
tactile and a force/torque sensor is described, illustrating some laboratory 
experiments of the technique discussed in Section 4.3. 

5.2.1 Measurement of the COR position. In general the COR position 
is not known and must be computed in real-time. Results obtained in this 
case are reported in Figure 5.3, reporting the displacement (note that this 
measure is a composition of both a rotational and a linear motion), the ‘slip’ 
signal, and the torques m/ (computed by the algorithm) and rrit (measured 
by the IT sensor). Figure 5.4 shows the computed COR position on the tactile 
pad. 



(a) 




Figure 5.3. Experiment with unknown COR position: (a) rotational displacement 
and result of condition (4.7) (1 = slippage); (b) torques m/ (solid) and nit (dotted) 



5.2.2 Rotational and translational slip control. Since it is possible to 
detect the slip between the two contacting bodies, it is possible to apply 
a control action similar to the one presented for the translational case. The 
condition to be checked is (4.7). For the sake of simplicity, we assume that the 
variation Sfn of the normal force /„ applied by the robotic finger is uniformly 
distributed on the contact area A, i.e. all the components change of the 
same amount 64>n = Sfn/N. Moreover, we define 6fn = Therefore, 

from (4.7) we obtain 



256 

nt < y^^fJ.sS(j>n\/iXi - Xc)^ + (j/, - l/e)^- 

i=l 



With the introduction of a conservative factor a, in case (4.7) does not 
hold we compute the desired variation 6fn of the normal force as 



SU 



nt — me 

I]i=i XcY + (yt- VcY 



(5.1) 
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Figure 5.4. COR position on the tactile pad 



Results obtained with the above control strategy are shown in Figure 5.5, 
from which it may be appreciated the increase of the grasping force (the 
normal force /„) when the external disturbance is applied, and the fact that 
slippage is stopped. 

Note that the fact of using both the sensors allows avoiding the rotational 
friction coefficient measurement, which depends on the size of the contact 
area A and therefore may change during task execution. Moreover, note that 
the object may be constrained using smaller values of the normal force (the 
technique based only on the IT sensor provides more conservative control 
actions) . 

5.3 Manipulation Exploiting Eviction 

The aim of the results discussed in this section represents a rather differ- 
ent approach to manipulation. The basic observation is that even in normal 
tasks the human beings normally exploit the friction in order to manipulate 
an object. Moreover, it can be observed that the capability of manipulating 
an object by taking advantage of friction may actually increase the dexterity 
of a given robotic device. Therefore, simple devices could be used to execute, 
although within certain limits, dexterous manipulations. For example, imag- 
ine a standard robotic gripper with only one degree of freedom: a grasped 
object could be ‘moved’ once it is grasped by exploiting the friction effect 
(and the external gravity force) by imposing a controlled slip. 

On the basis of the results illustrated in the previous sections for the 
detection of a rotational slippage, based on the comparison of the disturbance 
and the friction torque, the feasibility of executing a controlled slip of a 
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Figure 5.5. Rotational and translational slip control with both sensors: set point 
(a) and normal force (b), disturbance force (c) and torque (d); measured angle of 
displacement 



grasped object has been tested with the setup of Figure 5.1. The goal was to 
let a cylindrical object rotate of a given amount by properly decreasing the 
grasping force. 




A schematic drawing illustrating the experiment is reported in Figure 5.6. 
The object is grasped with a normal force /„ (controlled variable), and an ex- 
ternal constant force fa is applied. The value of the normal force is decreased 
until the comparison between the friction force and the external torque gen- 
erated by force fa indicates that a slip condition is generated. At this point, 
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a computation of the rotational movement of the object is performed and 
the normal force is changed accordingly. When the desired angular position 
is reached, the value of /„ is increased in order to firmly hold the object. 
This logic is summarised in Figure 5.7. Results of this approach are shown 
in Figure 5.8 and Figure 5.9. 

Note that in this simple case from the geometry of the grasp, assumed 
known, it is possible to write 



9 = arccos 



rriz 

U 



(5.2) 



where the force fa is constant (in the reported experiment fa = 0.86 N). It 
is therefore possible to determine the angular position 9 on the basis of a 
measure of the IT sensor. 




Figure 5.7. A slip control strategy 



The control strategy is summarised as follows, see also Figure 5.7: 

— the normal force /„ is decreased and a slip condition is detected by com- 
parison of the external and friction torque; 

— when the slip condition is detected, the current values of /„ and of 0, from 
(5.2), are saved; 
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— the value of 9 is compared with the desired set-point; if the error is less 
than a threshold, the normal force is increased, otherwise it is decreased. 

The results shown in Figure 5.8 and Figure 5.9 have been obtained by im- 
posing the initial normal force /„ = 4A1. The desired rotation is Od = 45 deg, 
and the value for the threshold in Figure 5.7 is £ = 0.5 deg. 



Normal force in the controlled slip 




In Figure 5.8 the external forces are shown. Initially, the normal force 
is decreased until the slip begins. At this point, the algorithm summarised 
in Figure 5.7 is applied. When the desired angular position is reached, the 
normal force is increased in order to stop the slippage. 



6 Conclusions 

This chapter has described the force and tactile sensing technologies and 
some of their applications in advanced robotic manipulation. Force and tac- 
tile sensors developed at the Universities of Bologna and Delft have been 
considered as signihcant examples, and have been used on two robotic setups 
for illustrating their effectiveness. The presented results refer to different as- 
pects of manipulation, from the estimation of the friction coefficient, to slip 
detection, to grasp control strategies aimed to minimising the grasp forces 
or optimising the hand/finger posture. Other uses of these sensors, reported 
only marginally, concern the implementation of force control strategies for 
dexterous manipulation. Moreover, research activities devoted to the inte- 
gration of the two devices in a single sensor and to further exploitation of the 
provided information have been presented. 
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Rotational slip 




Figure 5.9. Rotational motion of the grasped object 



From the results discussed here, representing only some aspects of the 
potentialities and use of these type of sensors, and that obviously only reflect 
the author’s point of view about these technologies, it clearly emerges that 
this field is still in a growing stage, and that further research is both necessary 
and of relevant interest for the robotic community, especially considering the 
current use of robot technologies in a number of different applications in non 
structured environments. 
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This chapter deals with control of robots with flexible links. The most recent 
results about modelling and control of flexible robots are examined; in par- 
ticular, the results obtained by the authors and concerning predictive control 
are described in detail. 



1 Introduction 



A mechanical structure with holonomic constraints can be represented by the 
following equations: 



B (q) q + h {q, q)+Kq + g (q) 



T 

0 



q&R^,T&R^ 



( 1 . 1 ) 



where B{q) is a symmetric positive definite matrix, h{q, q) includes the Cori- 
olis and centrifugal generalised forces, K is a constant positive (semi)definite 
matrix that model the elasticity, g{q) is the vector of gravitational generalised 
forces, T is the control vector. 

In case of robots with elastic joints and/or flexible links the number of 
control inputs is smaller than the number of Lagrangian coordinates m < 
n, making the system underactuated. In fact, the elements of the flexible 
links are coupled by the elastic forces/ torques Kq, which can be considered 
generated by proportional regulators with zero set points. Therefore, a flexible 
link without gravity has a unique equilibrium configuration of the neutral 
axis, determined only by the term Kq] in this case the elasticity reduces the 
degree of underactuation. The term g{q) can be regarded as a further input 
or a disturbance. 

In fact, the “underactuation” of the flexible robots is not of the most 
general kind. For instance, in case of rigid walking machines, only the gravity 
can be used as a further input; in case of rigid robots moving in a horizon- 
tal plane with some underactuated joints, not even the gravity is present, 
causing a “true underactuation” . Therefore, the wider class of underactuated 
holonomic systems comprises systems without gravity and elastic elements, 
and includes the class of flexible robots. 
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2 Modelling 

Modelling structures with flexible elements requires a preliminary examina- 
tion of a unique flexible element, since a distributed parameters body is more 
complex to be described than a rigid body. For this reason, in the following 
section some results in the analysis of a single flexible link are presented. 



2.1 Single Link 

The models are usually developed referring to the Saint Venant assumptions: 
i) the material behaves according the Hooke’s law; ii) the neutral axis is a 
straight line; iii) only small deflections are considered and the end point of 
the beam moves along a straight line orthogonal to the undeformed axis of 
the beam. In order to reduce the model complexity, it is usually assumed 
that the shear does not give contribution to the elastic energy and only the 
effect of the bending moment is considered; the rotational energy could not 
be considered in case of slender structures with small deflections; in case of 
beams moving in three dimensions, there is no coupling between the motion 
in orthogonal directions. The model employed to design the control law must 
be carefully chosen, since in some case these hypothesis are not verified. For 
instance, iii) is not verified for very slender structures like fishing-rods. 



2.2 Exact Model 



When all the above reported hypotheses are verified, the Euler Bernoulli 
equation of the single link can be derived: 



dx^ dx'^ 



d'^w (x, t) 



+ p{x,t) 



( 2 . 1 ) 



where w{x, t) is the deflection at abscissa x and time t, p{x, t) is the external 
load, Q is the mass per unit length, E is the Young’s modulus and J is the 
cross section momentum of inertia. This result can be obtained using the 
Hamilton principle. The same results can be obviously achieved using the 
Lagrange equation directly [44]. If the cross section inertia and the Young 
modulus are constant along the beam, it follows: 



d^w{x,t) d‘^w{x,t) 
dx^ 



= p{x,t) . 



( 2 . 2 ) 



Damping is not taken into account but it can be introduced by means 
of p{x, t) assuming a proper law. If no external loads act on the beam, the 
equation admits at least two kinds of solutions. 

Traveling waves — A possible solution is 



w {x, t) = f {kx ± u>t) = sin {kx ± ut) 



(2.3) 
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where k and uj can be determined substituting in (2.2). For instance, is the 
beam has length L and is hinged at the extremities, it follows: 



V = n 



L 




(2.4) 



The higher the freqnency, the higher is the wave speed. This particular solu- 
tion should be considered for slender structures, when the propagation delay 
is significant. 

The (differential) eigenvalues problem — The following stationary solu- 
tion is considered 

w (x,t) = 4> (x) '4> (t) . (2.5) 

Substituting in (2.2) yields 



EJ%p (t) 



d*<l> {x) 
dx^ 



p(j) (x) 



{t) 

dE 



= 0 



The solution for the eigenvalues problem is 



( 2 . 6 ) 



tp (t) = A sin (cat) + B cos (cat) 



(2.7) 



with the following eigenfunction(s) {(A = oj'^p/EJ): 



(j) (x) = Cl sin (Px) -I- C 2 cos (/3a;) -I- C 3 sinh (/3a;) -|- C 4 cosh (/3a;) . (2.8) 



The constants can be determined assuming the proper boundary conditions. 
The eigenfunctions are orthogonal with respect to the inner product and can 
be normalised: 

pL pL 

/ pi (x) pj {x)dx = 0 i ^ j pi{x) pi {x)dx = 1. (2.9) 

Jo Jo 

In case of a clamped beam the boundary conditions are (deflection and 
rotation at the clamped extremity, shear and moment at the other) 



w (0, t) = 0 



dw{0A) ^ Q ^j dJwjLA) ^ Q ^j d^wjLA) 
dx dx^ dx“^ 



0 Vt. (2.10) 



These boundary conditions are verified if the following characteristic equation 
holds: 



cos (/3T) cosh (/3L) = — 1 



( 2 . 11 ) 



which is satisfied by (infinite and numerable) Pi, determining the natural 
frequencies: 



Wi = Pi 



lEU 

P 



( 2 . 12 ) 



In case of a hinged beam the boundary conditions are (deflection and moment 
at the hinge, shear and moment at the free extremity) 
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w(0,t) = 0 EJ- 






= Q EJ 



cPw {L, t) 



dx'^ ” " dx^ 

with the following characteristic equation: 



= 0 EJ 



d'^w {L, t) 
dx'^ 



= 0 Vt 
(2.13) 



sin {(3L) cosh {(3L) = sinh {(3L) cos {(3L) . (2-14) 

which gives the natural frequencies, different from (2.11). The normalised 
eigenfunctions can be determined using the second of (2.9) and (2.10) 
or (2.13). Further details can be found in some classical books [44], [43], 
[59] or specihc articles, e.g. [9], [29], [55]. 



2.3 Rayleigh-Ritz Methods 

When the solution to the eigenvalue problem is not feasible or the closed 
form solution cannot be computed, approximate solutions can be obtained 
by means of the Rayleigh-Ritz methods. These methods assume a certain 
function to describe the shape of the neutral axis of the link. The following 
ones could be employed: 

Eigenfunctions — They must satisfy both the differential equations and 
the boundary conditions; 

Comparison functions — They are 4 times differentiable, satisfy all the 
boundary conditions but not necessary the differential equations; this class 
includes the eigenfunctions; 

Admissible functions — They are 2 times differentiable and satisfy only 
the geometrical boundary conditions; this class includes the comparison func- 
tions. 

The geometric boundary conditions reflect the geometric constraints; the 
natural ones reflect the constraints on forces and moments [44]. 

In the Rayleigh-Ritz method, the comparison functions of a complete-in- 
energy set should be employed. If the interest is focused in the approximate 
solution of the eigenvalue problem, the solution can be found in the space of 
the admissible solutions instead of the comparison functions. 

A series of polynomial can be employed, which can be properly trun- 
cated to a certain order. These solutions satisfy all the geometric boundary 
conditions but not the differential equations. 

OO 

w (x,t) = '^^f’i{t)x’‘. (2.15) 

i=0 

A suitable truncation is performed to determine the functions ifi ft) assum- 
ing the proper geometric boundary conditions. Further details can be found 
in [46], [56]. It can be proved that, notwithstanding this approximation does 
not verify the differential equations of motion, the solution converges to the 
exact one as the order of the truncation increases [27] . 
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2.4 Finite Element Method 



The finite element method can be regarded as a Rayleigh-Ritz method; in- 
stead of global admissible solutions extended over the entire domain (length) 
of the beam, the admissible functions are defined in subdomains (elements of 
the beam) and are usually low-degree polynomials [44] , [43] , [59] . 

The f-th element of length Li is characterised by the rotations and dis- 
placements at the extremities: 

'Li{t) = [zi Li9i z^+i (2.16) 



In case of bending vibrations, the Hermite cubics can be employed as 
interpolation functions; the functions in the z-th subdomain are 

<P, (x) = [3^^ - 2^3 1-3^^ + 3^^ + (2.17) 

where ^ = XijL\Xi G [0, TJ. The admissible function in the z-th subdomain 
is 

Wt{x,t) {x)<Lt.{t) (2.18) 

This function allows to compute the kinetic and elastic energy of the element: 






p{x) 



dwi {x, t) 

Wt 



dx = 'LT—'L,,. 



(2.19) 



U. it) = i JeJM = serfs'. (2.20) 

0 

Matrices Mi and Ki are constant positive definite and depend on the 
geometrical and mechanical parameters of the beam. The internal geometric 
boundary conditions are satisfied by imposing the congruence of the displace- 
ments and rotations at the element extremities. In practice the equations of 
the whole structure are obtained by a proper assembly of submatrices Mi 
and Ki. 

The method can be extended also to three dimensional cases and to trusses 
and frames. The hierarchical finite elements method combines the advantages 
of both the Rayleigh-Ritz and the finite element methods [43] . 



2.5 Multiple Links 

The exact model of a robot with a rigid link and a flexible forearm can 
be derived referring to the Hamilton principle, including the kinetic energy 
of the rigid link and the kinetic and potential energies of the flexible one. 
This approach can be generalised also to more complex structures, but the 
results are quite involved and are difficult to be employed for control design 
purposes. The model is described by a system of non linear partial differential 
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equations. An example is reported in [1]. Approximate models can be derived 
by means of the Rayleigh- Ritz method [58] . In the paper the authors develop 
the model of the rigid robot; the last link is modelled using the clamped- free 
eigenfunctions. 

The extension to multiple flexible link is plain, in theory. An example is re- 
ported in [11], where an automatic symbolic modelling method is introduced. 
In [15] the dynamics of multi-link spatial manipulators with flexible links 
and joints is modelled by a redundant Lagrangian/hnite element approach. 
The elastic deformations of the links are expressed in their tangential local 
floating frames. Both the rigid and elastic degrees of freedom of the system 
are treated as generalised coordinates, taking into account the coupling be- 
tween rigid body and elastic motion. The constraint equations, representing 
kinematical relations among different coordinates due to connectivity of the 
links, are added to the equations of motion of the system by using Lagrange 
multipliers. The model includes nonlinear ordinary differential equations and 
nonlinear algebraic equations depending on the coordinates and Lagrange 
multipliers. It can be converted into a set of differential equations, which is 
solved numerically to predict the dynamic behaviour of the system. 

Finally, a finite element approach can always determine a model almost 
automatically, also for three dimensional structures. The drawback is the 
huge dimension of the model. On the other hand a proper truncation could 
achieve smaller dimensions; in particular, examining the single body of the 
structure could give rise to suitable approximation also before the assembly 
process. 

2.6 Remarks 

The exact solution is useful to compute the “true” vibration frequencies of 
the beam. In control application it is usually approximated by some eigen- 
functions, derived assuming suitable boundary conditions. It should be noted 
that in case of multiple links, the natural boundary conditions depends on 
the control. The Rayleigh-Ritz methods allows dealing only with geometrical 
boundary conditions, which depend on the conhguration of the structure. For 
instance when two links are orthogonal, the joint can be considered a hinge; 
when the links are parallel, since deflection is possible, the hinge constraint 
is only an approximation, and the constrained extremities tend to be free as 
the stiffness of the first link decreases. The finite element approach can deal 
with all these cases; on the other hand, a large number of elements could 
be necessary to approximate the natural modes, giving rise to large order 
model. The model order can be reduced by means of low frequency approx- 
imations, but the employed algorithm should be carefully chosen, since the 
linearised model has the eigenvalues near the imaginary axis and could be 
non minimum phase. 
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3 Control 

3.1 PD Regulation 

The basis in control of robots with flexible links is the regulation of an equi- 
librium point. It can be shown that the decentralised PD regulation of the 
motors can achieve the asymptotic stability of the equilibrium point. The ef- 
ficiency of the algorithm (i.e. the damping that can be introduced), depends 
on the coupling between the motors and the link: only if the vibration of the 
flexible link causes the motors to move, damping can be achieved; therefore 
direct drive motors with colocated sensors are taken into account. Some pa- 
pers are devoted to show that a PD decentralised control law can impose 
and asymptotically stabilise a unique equilibrium point. In a first approach, 
reference is made to a two link robots with the last one flexible. The model 
includes the equations of the flexible beam in the Euler Bernoulli form and 
neither discretisation nor truncation are required. Assuming that no gravity 
is present, the global asymptotic stability of the unique equilibrium point 
can be proved by means of the Lyapunov method. The candidate Lyapunov 
function includes also the elastic and kinetic energy of the flexible beam and 
other terms related to the rigid motion. The LaSalle-Krasowskii theorem can 
be employed to prove asymptotic stability. The proof does not depend on the 
particular discretisation/truncation of the model equations and therefore is 
not sensitive to spillover. The result is also robust with respect to parameter 
uncertainties [1]. Only motor speed and position are required, but the proof 
can be easily extended to the case in which the PD regulator is replaced by 
a lead compensator; in this case only the motor position is necessary. In case 
of gravity a compensation term is introduced. The proof is reported in [14] 
where the damped and undamped case are examined. The compensation term 
can be computed either in the desired final position or along the trajectory. 
Since no feedback exists on the tip position error and only motor position and 
speed are employed, the payload and the masses must be known to achieve 
the desired point. 

3.2 Strain Feedback Control 

This technique is also employed in the regulation and includes in the control 
law some terms that depend on the strain of some points of the flexible 
links [41], [49], [28]. 

3.3 Inverse Dynamics 

In case of trajectory tracking, the inverse kinematics and/or dynamics of flex- 
ible robots is required. Some algorithms have been investigated. References 
can be found in [5], [8], [10]. 
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3.4 Fuzzy Regulation 

A colocated decentralised PD regulation can achieve global asymptotic sta- 
bility of the equilibrium point. On the other hand it is not possible to impose 
an arbitrary damping to all the modes. To improve the damping ratio and 
reduce the overshoot, it is possible to modulate the gains of the PD regulator 
in function of some outputs. A careful investigation of the behaviour of the 
plant can carry to the definition of rules to adapt the PD gains [17], [18], but 
the results strictly depend on the depth of the physical insight. The main 
property of this approach is that asymptotic stability can be demonstrated. 

The application to multi link flexible robots requires a huge set of rules 
and membership functions [45] . The problem can be overcome introducing an 
adaptation algorithm to synthesise and tune the rules on-line. Acceleration 
feedback of the end effector is required. The adaptation scheme is of MRAS 
kind. Wide explanations can be found in [45], [34], where the method is ap- 
plied to a two flexible link robot. Another application can be found in [25], 
where acceleration feedback is also required. Other references to fuzzy learn- 
ing control are [37], [35], [36]. 

3.5 Robust Control 

Hoo techniques are employed to design robust controllers for flexible links 
with uncertain flexible dynamics. If it is assumed that there is not damping, 
the link has all the eigenvalues on the imaginary axis, and therefore the 
necessary conditions to design an Hoo controller are not satished. Safonov [48] 
introduces a right shifting transformation in the s-plan to make the poles 
with real positive part. Since the Roo design does not cancel unstable poles, 
the controller is stable and remains stable after the inverse transformation. 
In practice the right-shifting allows only poor performance. Left-shifting is 
more feasible; on the other hand, since the regulator cancels the stable poles 
of the shifted plant, it will have imaginary zeros. Another approach consists 
in adopting a High Authority/Low Authority Control architecture: an inner 
loop makes the controlled system asymptotically stable, even if with poor 
performance (Low Authority Control); the external loop is designed referring 
to a more suitable system, in order to achieve the control system specifications 
(High Authority Control). Safonov [48] uses in the inner loop a PD regulator; 
Banavar [7] uses LQG controllers. The first approach employs a colocated PD 
regulator that is robust with respect to model uncertainties; the other method 
gives rise to better performance, because it implements a state feedback, but 
it is less robust in presence of relevant uncertainties; moreover it is sensitive 
to spillover; anyway, experiments show that the LQR regulator is effective. 
The Hoo controller can be designed using the Mixed Sensitivity Approach. A 
drawback consists in the fact that the whole system does not ensure zero error 
for step input. This is implicit in the Glover Doyle theorem, which requires 
that the input must be L2 integrable. A feedforward term could be employed, 
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but since it introduces implicitly a pole in the origin, the stabilising effect of 
the inner loop is lost. In practice the feedforward action can only reduce the 
steady state error. 

Another technique is named combined pole placement/sensitivity function 
shaping method [33], [32]. Reference is made to a discrete time representa- 
tion of the flexible link, the model of which is suitably truncated to represent 
the significant dynamics. After choosing the sensitivity and the complemen- 
tary sensitivity functions, an iterative procedure is employed to design the 
controller, achieving the desired specifications. 

The /r-techniques have also been applied. In this case, since structured 
uncertainties are taken into account, the controller is less conservative than 
an Hoo one. An example of application can be found in [30]. Improvements 
of the ^-techniques, developed for structural control applications, can be 
found in [60], where a mixed H 2 /H 00 technique is employed. Here, a set of 
mixed H 2 /H 00 compensators are designed, which are optimised for a fixed 
compensator dimension. The mixed norm recovers the H 2 design performance 
levels, while providing the same level of robust stability as in the n design. 

3.6 Singular Perturbation 

When the robot dynamics can be divided in fast and slow, the singular pertur- 
bation approach can be employed [20]. Firstly the robot is partially feedback 
linearised [52], improving also the separation between the slow and fast dy- 
namics. The fast dynamics depends on the flexibility and is considered as a 
perturbation of the slow dynamics, which coincides with the rigid motion. 
The controller is composed of a slow term that ensures tracking, and a fast 
term that damps the vibrations. Exponential stability can be demonstrated. 
A preliminary requisite is that the bandwidth for tracking must be well dis- 
tinguished from the vibration modes; from another point of view, either the 
robots is rather rigid or the required bandwidth for trajectory tracking must 
be small. Some references can be found in [4], where the technique is exper- 
imentally applied to a single flexible link. Applications to multilink robots 
can be found in [3], [64]. 

3.7 Slidiug 

The standard sliding mode technique requires to define a sliding surface for 
each Lagrangian coordinate; to drive the trajectories on the sliding surface 
the same number of control inputs are required. In case of flexible links, 
where there are more Lagrangian coordinates than inputs, it is required to 
adopt combined sliding surfaces. It is necessary to show that the equivalent 
motion converges to zero. This is easy in case of single arms [61], [62] or 
multilink robots, if the coupling between the links is negligible [12]. In case 
of two link robots with a rigid and a flexible link, it can be shown the dy- 
namics on the sliding surfaces is linear and can be stabilised by a suitable 
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choice of the parameters of the sliding surfaces [16]. The control law requires 
some deflection measurements on the flexible link. The control chattering is 
avoided approximating the ideal relay with a high gain saturated amplifier. 
Experimental tests can be found in [4]. Another approach is the continu- 
ous sliding- mode [63], which can be applied to minimum phase system. An 
example can be found in [26]. 



3.8 Input Shaping 

Shaped commands profiles are generated by convoluting a sequence of im- 
pulses with the desired command signal. One of the problem to be solved is 
the insensitivity, that is the ability of the input shaper to reduce the residual 
vibrations in presence of modelling error. Details and comparisons of some 
techniques are reported in [54] . Experimental results of the shaping technique 
are reported in [31], [13]. 

3.9 Cyclic Control 

This approach is useful when the motion occurs between some equilibrium 
points and the whole path is not specified [39] , [40] . Moreover, the problem of 
trajectory tracking for a flexible arm between two equilibrium points is made 
more difficult by the non-minimum-phase behaviour of this system. The prob- 
lem can be classified as a state steering one. On the basis of these motivations, 
the problem of steering the state of a control system by learning has been 
investigated theoretically. The algorithms are based upon the selection of a 
finite-dimensional subspace of the linear space or all control functions defined 
over a finite time interval. As the search for the steering control is restricted 
to this subspace, the resulting algorithm is finite dimensional [38]. 



3.10 Predictive Control 

Since in many applications the most of the vibration energy is not generated 
by disturbances but by the actuators [31], great advantages can be obtained 
by a feedforward action which, by suitable shaping of the input command, 
can improve the performance of an existing feedback controller. Feedforward 
controllers can be designed to implement an inverse model control scheme, 
aiming at the cancellation of the known dynamics of the open or closed loop 
system [51]; in this way, if a reliable model is available, perfect tracking of any 
trajectory can be obtained. The drawback of this approach is the difficulty 
in the design of general model inversion algorithms for non linear systems 
and in the lack of robustness. A problem which arises in the design of a feed- 
forward inverse hlter for linear flexible links is related to the fact that the 
transfer function between the motor and the tip is non minimum phase, im- 
plying an unstable inverse controller. This fact can be partially overcome by 
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designing a stable approximation of the inverse model [57]. Another method 
for the approximated model inversion is the Model Predictive Control ap- 
proach [24], Model Predictive Controllers (MPCs) plan on-line a suitable 
sequence of future input commands on the basis of a prediction model, in 
order to track a desired output trajectory and minimising a defined index of 
performance. Although MPCs have been mainly used in process control, they 
have been recently applied also to the control of mechanical systems char- 
acterised by non minimum-phase transfer functions and with badly damped 
poles [50], [2]. Another attractive feature of MPCs consists of their ability to 
handle constraints on control signals and state variables; these aspects are 
quite important because it is possible to generate sophisticated optimal con- 
trol laws satisfying multi-objective constrained performance criteria. Closed 
form MPCs are available only for linear systems minimising a quadratic cost 
function. Today analytical solutions are not available for general nonlinear 
MPC and efficient numerical procedures exist only for convex optimisation 
problems. In several papers Lagrange operators and quadratic programming 
have been used to manage nonlinear constrained optimisations. 

In MPC the optimal command sequence is determined as the result of an 
optimisation problem at each sampling instant. All the performance speci- 
fications are quantified by means of a cost index. An index, which is often 
employed, is: 

N2 Nc 

J = P \\y{k+j\k)-ydik + j)\\p + QY^\\Au{k+j-l)\\Q + Ji. (3.1) 

j=Ni j=l 

The first term of J evaluates the square error between the desired future 

output signal yd{k+j) and the predicted future output y{k+ j\k), estimated 
on the basis of the prediction model and the feedback information available 
at instant k. This error is evaluated over a defined prediction horizon, which 
is a window of N 2 — N\ samples. The second term of J weights the con- 
trol effort of the sequence of the input increments A{k) = u{k) — u{k — 1) 
over the control horizon window of samples. The diagonal matrices P 

and Q comprise weight coefficients, which are used to give a different em- 
phasis to the terms of J. The term Ji is a further cost function, which 
can be used to take into account other specifications or constraints. MPC 
is mainly used in conjunction with constraints on the amplitude and the 
rate of variation of the input and output variables. The minimisation of in- 
dex J is performed with regard to the sequence of the control increments 
[Au(k), Au{k -b 1), • • • , Au(k + Nu - 1)], j = 1, ..., Nu- 

A key aspect in predictive control is the application of the receding horizon 
strategy. In this approach only the first sample of the optimal sequence is 
applied to the system; subsequently the horizon is moved one step in the 
future and the optimisation is repeated on the basis of the measured feedback 
information. 
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The computational load required by classical optimisation algorithms with 
a great number of decision variables is the main difficulty encountered in non- 
linear constrained MFC design. A way to overcome these problems is to fuse a 
MFC scheme with intelligent control optimisation techniques. In this context 
a very promising approach is to exploit Evolutionary Algorithms (EAs) . EAs 
are pseudo random parallel search algorithms, the inference engine of which 
is based on the principle of biological evolution. One of the main advantages 
of evolutionary optimisation is the only linear increase of computational time 
with the number of decision variables; the main disadvantage, when employed 
in feedback control, is the lack of results proving the stability of the control 
law; on the other hand EAs showed to be quite appropriate optimisers for 
MFC [47], [42]. In [21] an Evolutionary MFC (EMFC) is used as feedforward 
controller in order to improve the trajectory tracking accuracy of a feedback 
controlled mechanical system. In [22] a FD-t-iLoo controller has been employed 
(Figure 3.1), while in [23] only a FD regulator. The aim of the feedback loop 
is to achieve the asymptotic stability even if with poor performance. To guar- 
antee a certain robustness, some information from the plant can be employed 
(realigment): the actual output error can be fed back and included in the op- 
timisation index; the nominal model used for the prediction can be simulated 
starting from the measured or estimated state of the plant; in this way the 
nominal model behaves like a one step predictor (Figure 3.2). 
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Figure 3.1. Pure Feedforward MPC 



The complete freedom in the choice of the multivariable objective function 
and the possibility to handle constraints allow the fulfillment of additional 
performance of the whole control system. 

As far as the real time implementation is concerned, in [23] some tests 
were carried out on an experimental set-up. In order to achieve the desired 
performance, the Evolutionary algorithm must be carefully tuned, with suit- 
able genetic operators and parameters. An increase in the computational 
speed can be achieved by a proper coding of the decision variables, but a 
deeper insight in the optimisation problem and in the genetic algorithm can 
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Figure 3.2. MFC with feedback information 



give rise to a better performance. In particular, two improvements can be 
introduced. 

Steady state reproduetion mechanism during the optimisation for the k-th 
sample — Since a limited computation time is available, it is essential that 
the good solutions founded during previous evolutions are not lost, but are 
used as “hot starters” for the next generation. To implement this strategy, 
the best chromosomes pass unchanged from the current generation to the 
next one, ensuring a not decreasing fitness function for the best individual 
of the population. The remaining part of the population is generated on the 
basis of a rank selection mechanism: a certain number of the best individ- 
uals constitute a mating pool; within the mating pool two random parents 
are selected and two children are created applying crossover and mutation 
operators; this operation lasts until the new population is entirely generated. 

Heredity from time interval k to k+1 — The results of optimisations at the 

instant are not wasted, but they constitute part of the starting popula- 
tion for the next time step optimisation. This approach can be easily applied 
in a receding horizon strategy; in fact the best chromosomes of the current 
generation computed at the end of the fc-th time interval can be exploited 
as a privileged starting solution for the next time step optimisation. At the 
beginning of the {k -f l)-th time interval, since the horizon is shifted one 
step ahead, the N genes of the best solutions are shifted forward to the next 
location; in this way the first genes are lost and replaced by the second ones 
and so on; the values of the last positions are filled by keeping unaltered the 
corresponding values of the previous optimisation. The remaining chromo- 
somes are randomly generated. The application of hereditary information is 
of great relevance, because a dramatic improvement in the convergence speed 
of the algorithm can be achieved and a good updated solution can be found 
in few generations [22], [22], [23]. 

Some results are reported in the subsequent figures. Reference was made 
to a single flexible link. The controller must damp the oscillations when the 
link starts from a deformed condition. In Figure 3.3 the free response is re- 
ported and compared with a well tuned PD controller. In Figure 3.4 a pure 
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Figure 3.3. Free and PD controlled flexible link 



feedforward MPC is employed. An increase in damping is achieved, but the 
mismatches between the nominal model employed for the prediction and the 
real plant cause some residual oscillations. In fact the prediction model is 
controlled to zero, contrary to the real plant. Finally, in 3.5 an intermittent 
realignment is introduced; the residual oscillation are very small, but they are 
not disappeared; this effect depends on the evolutionary algorithm that can- 
not generate a zero command signal, because of the quantisation. A further 
improvement consists in introducing an adaptation mechanism, which reduce 
the quantisation effects as the error decrease, causing the residual oscillations 
to vanish. 




Figure 3.4. Flexible link with pure feedforward MPC 
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Figure 3.5. Flexible link with intermittent realignment 



4 Conclusion 

A brief survey on control of flexible robots has been reported. The literature 
about these arguments is so huge that only some citations have been included. 
For a deeper insight, reference could be made to the bibliography hereafter. 
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In the framework of interaction control of robotic systems, impedance control 
represents one of the most effective strategies when the model of the envi- 
ronment is not available. In this chapter, a new impedance control strategy 
is presented for six-degree-of freedom (six-DOF) tasks. The main features 
are geometric consistency and absence of singularities. The case of a sin- 
gle manipulator interacting with the environment is considered first. Then, 
the case of redundant manipulators is analysed, and an algorithm ensuring 
stabilisation of null-space velocities is presented. The redundant degrees of 
mobility are exploited to optimise an additional task function. Finally, the 
case of cooperative robots manipulating a common object is addressed: both 
the problems of loose and tight cooperation are considered. The theoretical 
findings are validated in experiments on a dual-robot industrial setup. 



1 Introduction 

Control of interaction between a robot manipulator and the environment 
is crucial for successful execution of a number of practical tasks where the 
robot end effector has to manipulate an object or perform some operation 
on a surface. Typical examples include polishing, deburring, machining or 
assembly. During interaction, the environment sets constraints on the geo- 
metric paths that can be followed by the end effector. In such a case, the use 
of pure motion control for controlling interaction is a candidate to fail, while 
it is expected that enhanced performance can be achieved with an interaction 
control provided that force measurements are available [25]. 

Interaction control strategies can be grouped in two categories; those per- 
forming indirect force control and those performing direct force control. The 
main difference between the two categories is that in the former force control 
is achieved via motion control, without explicit closure of a force feedback 
loop; in the latter, instead, the contact force is controlled to to a desired 
value, thanks to the closure of a force feedback loop. 

To the first category belong compliance control [21], stiffness control [23] 
and also impedance control [14], where the position error is related to the 
contact force through a mechanical stiffness or impedance of adjustable pa- 
rameters. 

If a detailed model of the environment is available, a widely adopted strat- 
egy belonging to the second category is the hybrid position/force control [22], 
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[27], [17], [18]. In most practical situations, the model of the environment is 
not available; in such a case, effective strategies still in the second category 
are the inner/outer motion/force control [12] and the parallel control [10]. 

The focus of the present chapter is on impedance control for six-degree-of- 
freedom (six-DOF) tasks. Impedance control is aimed at imposing a desired 
dynamic behaviour to the end effector of a robot manipulator in the presence 
of external forces and moments, described by a mechanical impedance. For 
three-degree-of- freedom tasks (i.e. involving the sole end-effector position), 
the impedance consists in three linear second order differential equations 
corresponding to a mechanical system with desired mass, damping and stiff- 
ness. If a minimal representation of the end-effector orientation is adopted 
(e.g. Euler angles), the generalisation of the impedance to six-DOF tasks may 
result to be geometrically inconsistent; moreover, representation singularities 
may occur. In order to guarantee geometric consistency, the rotational part 
of the impedance equation is expressed in terms of a class of geometrically 
meaningful orientation representations based on the equivalent axis and an- 
gle of rotation. Singularities are avoided by using the unit quaternion, which 
is a four parameter angle/axis representation. An energy-based formulation 
is adopted to derive the impedance equation whose rotational part results to 
be nonlinear [7]. 

The geometrically consistent approach is applied also to the impedance 
control of redundant manipulators, characterised by a number of degrees 
of mobility greater than the number of degrees of freedom required for the 
execution of a given task. The method adopted for redundancy resolution 
allows complete decoupling of internal motion control from forces and mo- 
ments acting on the end effector. Thus internal motion can be exploited to 
meet additional task requirements, e.g. to improve robot dexterity, without 
affecting or being affected by the execution of the interaction task [20]. 

In the framework of interaction control of cooperative robots manipulat- 
ing a common object, both the problems of loose and tight cooperation are 
addressed. 

A cooperative control strategy can be termed loose when the manipulation 
task is executed by controlling each robot in an independent fashion. Coop- 
eration is realised only at the task planning level, e.g, mating rigid parts such 
as a dual-robot assembly in a workcell. In this case limited mating forces are 
achieved by adopting a geometrically consistent impedance control strategy 
for one of the two manipulators [5] . 

A cooperative control strategy can be termed tight when the manipulation 
task is executed by controlling the robots in a coordinated fashion. Cooper- 
ation is realised not only at the task planning level, but also at the control 
level. This is the typical task of two robots whose end effectors tightly grasp 
a commonly held rigid object, thus creating a closed-kinematic chain. In this 
case, interaction of the grasped object with the environment is successfully 
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managed by enforcing a geometrically consistent impedance behaviour at the 
object level [4], 

The control strategies presented in this chapter have been experimentally 
tested on a set-up composed by two industrial robots Comau SMART-3S each 
of them equipped with a force/torque sensor ATI FT-30/100 mounted at the 
wrist. A number of experiments for representative robotic tasks is illustrated. 



2 Motion Control 

In this section the kinematic and dynamic modelling of a robot manipulator 
are briefly presented. Moreover, the motion control problem in the task space 
is analysed, which is the base for developing interaction control. 

2.1 Modelling 

The kinematic model of an open-chain robot manipulator gives the relation- 
ship between the (n x 1) vector of joint variables and the (3 x 1) position 
vector Pg and the (3 x 3) rotation matrix He, i.e. — p^io) and Re — Reio)- 
The quantities Pg, Re characterise the end-effector frame Se{Oe-XeYeZe) 
with respect to a fixed base frame Sb{Ob-XbYbZt) and no superscript is used; 
instead, if a matrix or vector quantity is to be referred to a frame other than 
the base frame, then a proper frame superscript shall precede the quantity. 

Let q denote the vector of joint velocities, Pg the vector of end-effector 
linear velocity, and uje the (3x1) vector of end-effector angular velocity. 
The differential kinematics model gives the relationship between q and Ve = 
[pj in the form 

Ve = J{q)q (2.1) 

where J is the (6 x n) end-effector geometric Jacobian matrix. A general 
motion task for the end-effector position and orientation requires m degrees 
of freedom with m < 6. Whenever the number of joints exceeds the number 
of degrees of freedom, i.e. n > m, the manipulator is said kinematically 
redundant. 

The dynamic model can be written in the form 

B{q)q + C(q, q)q + Fq + g{q) = r - J^{q)h, (2.2) 

where B is the (6 x 6) symmetric positive definite inertia matrix, Cq is the 
(6 X 1) vector of Coriolis and centrifugal torques, g is the (6 x 1) vector 
of gravitational torques, r is the (6 x 1) vector of driving torques, h = 
[f'^ is the (6x1) vector of contact force exerted by the end effector 

on the environment. 
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2.2 Motion Control in Task Space 

The motion control problem for a robot manipulator can be formulated as 
finding the joint torques which ensure that the end effector attains a desired 
position and orientation. Since the present chapter is focused on the problem 
of controlling the interaction between the manipulator end effector and the 
environment, direct feedback of task-space variables (i.e. end-effector position 
and orientation) is utilised. 

A classical control strategy is inverse dynamics control, which is aimed at 
linearising and decoupling the manipulator dynamics via feedback. The joint 
torques are chosen as 

T = B{q)a + C{q, q)q + Fq + g{q) + {q)h (2.3) 

where a is a new control input to be properly designed. 

Folding the control law (2.3) into the system model (2.2), and taking into 
account that B{q) is always nonsingular, yields 

q = a (2-4) 

which constitutes a linear and decoupled system, where the quantity a rep- 
resents a resolved acceleration in terms of joint variables. 

The new control input a. in (2.4) can be chosen as 

a = J~\q) (a- j{q,q)q^ (2.5) 

which leads to 

Ve = a (2-6) 

where the time derivative of (2.1) has been used. The vector a attains the 
meaning of a resolved acceleration in terms of task space variables. 

In deriving (2.5), a nonredundant manipulator (n = 6) moving in a singu- 
larity-free region of the workspace has been considered to compute the inverse 
of the Jacobian. 

It is appropriate to partition the vector a into its linear and angular 
components, i.e. a = [aj where and ag are (3 x 1) vectors. 

Therefore, Equation (2.6) can be rewritten as 

Pe = (2-7) 

u)q — cioj (2-8) 

where Op and ag shall be designed so as to ensure tracking of the desired 
end-effector position and orientation trajectory, respectively. 

The desired position trajectory is specified in terms of the position vec- 
tor linear velocity vector and linear acceleration vector Pj_{t)-, 
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the desired orientation trajectory is specified in terms of the rotation ma- 
trix Rd{t), angular velocity vector u>d{t) and angular acceleration vec- 
tor d>d{t). The quantities and Rd characterise the origin and the ori- 
entation of a desired frame Ed- 

A position error between the desired and the actual end-effector position 
can be defined as — p^^—p^ where the operator A denotes that a vector 
difference has been taken, and the double subscript denotes the corresponding 
frames. Then, the resolved linear acceleration can be chosen as 

ap = Pd + K DpApa^ + K ppAp^^ (2.9) 

ensuring exponential tracking of the desired position trajectory for any choice 
of the positive definite matrix gains Kpp and Kpp. 

The most natural way of defining an orientation error is to consider an 
expression analogous to the position error, i.e. Aip^^ — — where (p^ 

and are the set of Euler angles that can be extracted respectively from 
the rotation matrices Rd and Re describing the orientation of Ed and Ee- 

The resolved angular acceleration based on the Euler angles error can be 
chosen as 

= T{ip^){<p^ + KpoA^ele + KpoAip^^) + T{ip^, (Pe)‘Pe (2.10) 

where T is the transformation matrix relating the time derivative of the Euler 
angles ipe and the end-effector angular velocity Wg, i.e. 

^e=T{ip^)<p^. (2.11) 

The control law (2.10) ensures exponential tracking of the desired orientation 
trajectory for any choice of the positive definite matrix gains Kp>o and Kpo, 
provided that T{(p^) is nonsingular. 

The above Euler angles error becomes ill-conditioned when the actual 
end-effector orientation (p^ becomes close to a representation singularity, i.e. 
a configuration for which T{ip^) becomes singular. In order to overcome this 
drawback, an alternative Euler angles error can be considered which is based 
on the rotation matrix describing the mutual orientation between Ed and Ee, 
i.e. ^Rd = R^Rd Let ip^e denote the set of Euler angles that can be extracted 
from ^Rd- In this case, the resolved angular acceleration can be chosen as 

a„ = Cbd + Te{<Pde){KDo<fde + ^ PoP>de) ~ Te{^de, <Pde)<Pde (2-12) 

where Tg = ReT. The control law (2.12) ensures exponential tracking of the 
desired orientation trajectory for any choice of the positive definite matrix 
gains Kpo and Kpo, provided that T(t^^g) is nonsingular. The advantage 
of the alternative over the classical Euler angles error is that, by adopting a 
representation (p^^ for which T(0) is nonsingular (e.g. the XYZ Euler angles), 
representation singularities occur only for large orientation errors. 
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A different definition of orientation error can be obtained using an an- 
gle/axis representation. The mutual orientation between Sd and Ag is de- 
scribed by ^Rd: and thus the orientation error can be defined in terms of the 
general expression 

^Ode = fi^de^rde, (2.13) 

where 'dde and are respectively the rotation and the unit vector cor- 
responding to and /(•) is a suitable scalar function with /(O) = 0. 

Common choices for f{'&) are reported in Tab. 2.1. 



Representation 




Classical angle/axis 


sin(il) 


Quaternion 


sin(i?/2) 


Rodrigues parameters 


tan('!?/2) 


Simple rotation 


1? 



Table 2.1. Common choices for /(i9) 



A special type of angle/axis representation of the orientation error is 
obtained with the quaternion, i.e. 

^Ode = sin = ‘'ede (2.14) 

corresponding to the vector part of the quaternion Qde = {r]de,'^£de} that 
can be extracted from the rotation matrix A brief review of the basic 
properties of the unit quaternion can be found, e.g. in [25]. 

The resolved angular acceleration based on the quaternion error can be 
chosen as 

do — ^d T R de T R PoRe ^de (2-lb) 

where AuJde = <^d — is the relative angular velocity between Ed and 
Ee- Control law (2.15) ensures asymptotic tracking of the desired trajectory 
when the feedback gains are taken as scalar matrices, i.e. Koo = kool and 
Kpo = kpol [ 6 ]. 



3 Six-DOF Impedance Control 

When the manipulator moves in free space, the end effector is required to 
match a desired frame Ed- Instead, when the end effector interacts with the 
environment, it is worth considering another (compliant) frame Ec specified 
by Po and Rd then, a mechanical impedance can be introduced which is 
aimed at imposing a dynamic behaviour for the position and orientation 
displacements between the above two frames. 

In the following, six-DOF impedance control schemes are derived using 
different types of orientation displacements. 
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3.1 Classical Impedance Control 

The mutual position between the compliant frame and the desired frame can 
be described by the position displacement — p^ — p^. The impedance 
equation is typically chosen so as to enforce an equivalent mass-damper-spring 
behaviour for the end-effector position displacement under an external force 
/ acting on the end effector 

MpAp^^ + DpAp^^ + KpAp^^ = f, (3.1) 

where Mp, Dp and Kp are positive definite matrices. 

In order to ensure a proper end-effector behaviour for the successful exe- 
cution of an interaction task, the selection of the stiffness matrix plays a key 
role. Therefore, it is worth analysing the elastic term from a geometric point 
of view. The stiffness matrix Kp can be decomposed as Kp — UpPpUp 
where Fp = diagjypi, 7 ^ 2 , Tps} and Up = [ttpi Up 2 itps] are respectively 
the eigenvalue matrix and the (orthogonal) eigenvector matrix. Then, con- 
sidering a position displacement of length A along the Tth eigenvector Upi 
leads to an elastic force 



f E — ^P^Pdc — 'Ypi^'^pi (3-2) 

which is aligned with the same axis. This implies that the translational stiff- 
ness matrix can be expressed in terms of three parameters 7 pi representing 
the stiffness along three principal axes ttpi, and in turn it establishes the 
property of task geometric consistency for the elastic force in (3.2). 

The end-effector orientation displacement can be computed as a difference 
of Euler angles, i.e. A(f)^^ — (f)^ — 0^, where 4>^ and 0^ denote the set of Euler 
angles corresponding to Rc and Rd, respectively. In this case the rotational 
part of the impedance at the end effector can be formally defined in the same 
way as for the positional part (3.1), i.e. 

MoAipdc + DoAip^^ + KoAif^^ = (3.3) 

where Mo, Do, Ko are positive definite matrices describing the generalised 
inertia, rotational damping, rotational stiffness, respectively, and p is the 
contact moment at the end effector; all the above quantities have been re- 
ferred to the base frame. Notice that, differently from (3.1), the dynamic 
behaviour for the rotational part is not absolutely determined by the choice 
of the impedance parameters but it does also depend on the orientation of the 
compliant frame with respect to the base frame through the matrix {(po). 
Moreover, Equation (3.3) becomes ill-defined in the neighborhood of a repre- 
sentation singularity; in particular, at such a singularity, moment components 
in the null space of T'^ do not generate any contribution to the dynamics of 
the orientation displacement, leading to a possible build-up of large values of 
contact moment. The effect of the rotational stiffness can be better under- 
stood by considering an infinitesimal orientation displacement between Sd 
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and Ec- From (3.3), in the absence of representation singularities, the elastic 
moment is {(p^)KoA(p^^. In the case of an infinitesimal orientation 

displacement about ip^, it is 

= (cpd - dt = T~^(V3jZ\wdcdt, (3.4) 

¥>d = V>c 

where Au>dc = — <^c is the relative angular velocity between Ed and Ec- 

Hence, the elastic moment for an infinitesimal displacement d(Z\(^dc) 

tiE = T-'^{p,)K,T-\<p^)Au:dcdt. (3.5) 

Equation (3.5) reveals that the relationship between the orientation displace- 
ment and the elastic moment depends on the orientation of Ec- It follows 
that the property of task geometric consistency of the elastic force (3.2) is 
lost, that is, the eigenvectors of the matrix Ko do not represent the three 
principal axes for the rotational stiffness. 

The drawbacks discussed above can be mitigated by adopting the alter- 
native Euler angles displacement <Pdc that can be extracted from the rotation 
matrix Then, a rotational impedance at the end effector can 

be defined as 

MoPdc + DoiPdc + KoPdc = T^{VdcY^J‘ ( 3 - 6 ) 

where Mo, Do and Ko are defined in a similar way to (3.3) and is re- 
ferred to Ec- An advantage with respect to (3.3) is that now the impedance 
behaviour for the rotational part depends only on the relative orientation be- 
tween Ed and Ec through the matrix {pdc)- Hence, if XYZ Euler angles 
are adopted, representation singularities have a mitigated effect since they 
occur for large end-effector orientation displacements. From (3.6) the elastic 
moment is {ifdc)^oPdc- The infinitesimal orientation displace- 

ment about ipdc = 0 is 

d¥’dc = ‘Pdc dt = T^^(0)Z\‘=a;dcdt. (3.7) 

Vdc = 0 

Hence, the elastic moment for an infinitesimal displacement dt^^c is 

= T-^{Apdo)KoT-\Q)A^u:dAt = T-^{Q)KoT-\Q)A^uJdAi (3.8) 

where the first-order approximation T^"^(d<Pdc)dt — T^"^(0)dt has been 
made. Equation (3.8) reveals that the relationship between the orientation 
displacement and the elastic moment is independent of the orientation of Ec- 
Notice, however, that the choice of Euler angles affects the resulting stiffness 
through the matrix T(0) which must be invertible. It is convenient to adopt 
the XYZ representation, which gives T(0) = I and thus, for an infinitesimal 
displacement. 



/xg ~ KoA'^bJdcdi. 



(3.9) 
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As regards the property of task geometric consistency for the elastic moment, 
when Ko is a diagonal matrix and the XYZ representation of Euler angles 
is adopted, the i-th eigenvector Uoi of Kg — diag{7oi, 7 o 2 , Tos} is the i-th 
column of the identity matrix. Hence, the orientation displacement of an 
angle '&dc about Ugt is described by = ^dcUoi, which, in view of the 
expression of T{(p^g) for XYZ Euler angles [25], leads to 

(3.10) 

representing an elastic moment about the same Ugi axis; thus the vectors Ugi 
have the meaning of rotational stiffness principal axes. It can be recognised 
that the same property does not hold in general for a nondiagonal Kg [7]. 

3.2 Geometrically Consistent Impedance Control 

The above analysis has revealed that the adoption of minimal representations 
of orientation for the rotational part of the impedance does not preserve 
properties analogous to those of the translational part. The main objective of 
this section is to define a six-DOE impedance with a rotational part matching 
the following desirable properties: 

— the velocity used in the impedance equation should be dual to the mo- 
ment fjb acting on the end effector, i.e. with no need of a transformation 
matrix depending on the actual end-effector orientation; 

— the equivalent rotational stiffness in case of small orientation displacements 
should be always well dehned; 

— the elastic contribution should allow the specification of a rotational stiff- 
ness matrix in a consistent way with the task geometry. 

A class of geometrically meaningful representations of the mutual orienta- 
tion between Ed and Eg can be given in terms of the angle/axis displacement 

‘'odc = fi'ddcYrdc, (3.11) 

where "&dc and ‘^rdc correspond to '^Rd, and fi'&dc) is any of the functions 
listed in Tab. 2.1. Those are strictly increasing smooth functions in an inter- 
val with > 0. Hence, the derivative f'i'&dc) of / with respect 

to 'ddc is strictly positive in that interval. 

Differentiating (3.11), gives [7] 

^ddc = f2Crdc,^dc)A^i^dc (3.12) 

with 

^ = /'(t?dc)Vrf/rI+i/(t^,,)(cot(d,,/2)(/- Wrl)-5(^rrf,)), (3.13) 

where S{-) is the (3 x 3) skew-symmetric matrix operator performing the 
cross product. Notice that the following property of f2 holds 
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f2Crdc,0) = f'{0)I (3.14) 

which will be useful in the following. 

In order to derive the impedance equation for the rotational part, it is 
convenient to refer to the following energy-based argument. Let 

% = (3.15) 

express a rotational pseudo-kinetic energy of a rigid body with inertia tensor 
Mo and angular velocity A'^uSdc- Then consider the potential energy 

Uo = %p‘"OdoKo‘'odc, (3.16) 

where ip is a positive constant depending on the particular choice of /(•), and 
Ko is a symmetric positive definite matrix. Having defined the various energy 
contributions, the terms in the rotational impedance equation can be derived 
by considering the associated powers. Taking the time derivative of (3.15) 
yields 

to = Vjz\W (3.17) 

where 

= MoA^Udc (3.18) 

is the inertial moment. Further, taking the time derivative of (3.16) and 
accounting for (3.12) yields 

Uo = ^filA^u^dc (3.19) 

where 

= 2ipf2'^Crdc, '&dc)Ko^Odc (3.20) 

is the elastic moment. Finally, a dissipative contribution can be added as 

= DoA^LJdc, (3.21) 

where Do is a positive definite matrix characterising a rotational damping at 
the end effector. 

Therefore, a rotational impedance at the end effector can be defined by 
adding the contributions (3.18), (3.21) and (3.20), i.e. 

M oA'^uidc + DgA'^LiJdc + K'o‘^Odc = (3.22) 

where the equality -1- has been imposed, and 

K'o = 2ipf2^{‘^rdc,^dc)Ko. (3.23) 

Notice that the rotational part of the impedance equation has been de- 

rived in terms of quantities all referred to Ec', this allows the impedance 
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behaviour to be effectively expressed in terms of the relative orientation be- 
tween Sd and Ec, no matter what the absolute orientation of the compliant 
frame with respect to the base frame is. It is worth remarking that the ve- 
locity used in the impedance equation is dual to the moment exerted by 
the end effector, i.e. there is no need of a transformation matrix depending 
on the actual end-effector orientation. 

In the following, the analysis for small orientation displacements is car- 
ried out and consistency with the task geometry is investigated. Consider an 
infinitesimal orientation displacement expressed as 



d'^Odc 



‘'Odc 



n 0)Z\‘'a;dcdt = f'(0)Z\‘'a;dcdt 

Vdc = 0 



(3.24) 



where the property (3.14) has been exploited. Folding (3.24) into (3.20), 
written for an infinitesimal displacement about 'ddc = 0, gives 

‘'He = 2^pf2^{‘'rdc,d'&dc)Kod‘^Odc (3.25) 

~ K.A^^uJdcdt = Ko4i"^dcdt, (3.26) 



where the first-order approximation i7('^rdc, d'ddc) — ff(0)/ has been consid- 
ered and the choice ■)/> = l/2(/'(0))^ has been made. Equation (3.25) clearly 
shows how the relationship between the orientation displacement and the 
elastic moment is independent of the orientation of Ec, and the problem of 
representation singularities is not of concern since f{0) is finite. 

As regards the property of task geometric consistency, the stiffness 
matrix in (3.20) can be decomposed as Kg — UorgUj, where Fg = 
diag{ 7 oi, 7 o 2 , 7 o 3 } and Ug = [ugi Ug2 Mos] are respectively the eigen- 
value matrix and the (orthogonal) eigenvector matrix. Then, considering an 
orientation displacement by an angle ddc about the t-th eigenvector 



^dc — f {.d dc)'^oi, 

and taking into account the expression of fl in (3.13), yields 

= 2ll)f'{'ddc)f{ddc)loiUoi. 



(3.27) 

(3.28) 



This represents an elastic moment about the same Ugi axis which is in the 
same direction of the orientation displacement since fi^dc) > 0. Therefore, 
the rotational stiffness matrix can be expressed in terms of three parame- 
ters 7 oi representing the stiffness about three principal axes Ugi, i.e. in a 
consistent way with the task geometry. 

A special case among angle/axis representations of orientation displace- 
ment is constituted by the quaternion displacement. Such a representation has 
the advantage over other angle/axis representations to avoid representation 
singularities. The mutual orientation between Ed and Ec can be described by 
the quaternion Qdc = {?7dcAcdc} extracted from Indeed, the orientation 
displacement to be considered in (3.22) is given by the vector part '^edc and, 
in view of (3.16), the expression of the potential energy becomes 
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Uo = 2^eJ^Ko^€dc, (3.29) 

where it has been set %[> = 2. Even though the potential energy is expressed in 
terms of the vector part of the quaternion, it can be shown that Uo coincides 
with the rotational elastic energy associated with a torsional spring of stiffness 
Ko acting so as to align Sc with Sd [7] . 

In view of (3.22), the resulting impedance equation for the rotational part 
becomes 

M o^SuSdc + D o^SuJdc + K'J^edc = (3.30) 

where the rotational stiffness matrix is 

K'c = 2E^{ijdc,^edc)Ko (3.31) 

where E = r]dcl - Si^^Cdc)- 

A generalisation of the proposed approach leads to an impedance equation 
characterised by coupling elastic terms between the translational and the 
rotational part; they may be useful for the execution of assembly tasks. The 
details can be found in [8]. 

3.3 Inner Motion Control Loop 

The impedance behaviour at the end-effector can be enforced by resorting to a 
model-based control law designed so as to give closed-loop dynamic equations, 
for both position and orientation, coinciding with those describing the desired 
impedance [24]. However, the selection of good impedance parameters that 
guarantee a satisfactory compliant behaviour during the interaction may turn 
out to be inadequate to ensure accurate tracking of the desired position and 
orientation trajectory when the end effector moves in free space. A solution 
to this drawback can be devised by separating the motion control action 
from the impedance control action as follows. The motion control action is 
purposefully made stiff so as to enhance disturbance rejection but, rather 
than ensuring tracking of the desired end-effector position and orientation, 
it shall ensure tracking of a reference position and orientation resulting from 
the impedance control action [Ij. In other words, the desired position and 
orientation together with the measured contact force and moment are input 
to the impedance equation which, via a suitable integration, generates the 
position and orientation to be used as a reference for the motion control 
action. 

In detail, the control input ap in (2.6) has to be designed to match the 
desired impedance for the translational part and the rotational part, respec- 
tively. In view of (3.1), ap is taken as in (2.9), by simply replacing the variables 
referring to the desired frame (subscript d) with the variables referring to the 
compliant frame (subscript c). The latter can be computed by forward inte- 
gration of the translational impedance equation (3.1) with input / available 
from the force sensor. 
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As regards the orientation loop, Oq can be chosen according to the differ- 
ent representations of orientation displacement illustrated above. Similarly 
to what has been conceived for Op, the control input Oq is taken as in (2.10), 
(2.12) or (2.15) by simply replacing the variables referring to the desired frame 
(subscript d) with the variables referring to the compliant frame (subscript 
c). The latter can be computed by forward integration of the corresponding 
rotational impedance equations (3. 3), (3. 6) or (3.30) with input /jl available 
from the force sensor. 

3.4 Redundancy Resolntion 

In case the manipulator is kinematically redundant, there exist infinite joint 
motions that produce the same end-effector motion. In particular, even when 
the end effector is at rest, it is possible to generate an internal motion at the 
joints. As a minimal requirement, such motion should be made stable [16]. 
In addition, it could be exploited to meet additional task requirements be- 
sides the execution of the end-effector trajectory, thus providing redundancy 
resolution [19]. 

Redundancy can be solved either at kinematic level, that is in the first 
stage of a kinematic control strategy, or at dynamic level by suitably modi- 
fying the inverse dynamics control law [13]. The latter approach is pursued 
hereafter. 

Since the Jacobian matrix for a redundant manipulator has more columns 
than rows (n > 6), a suitable right inverse of J is to be used in lieu of J~^ . 
Hence, in lieu of (2.5), the new control input in (2.3) can be chosen as 

a = (q) (^a - j {q)q^ + ctn (3.32) 

where 

Jt = (3.33) 

denotes the right pseudo-inverse of J weighted by the positive definite (n x n) 
matrix W. Also, in (3.32), denotes a joint acceleration vector lying in the 
null space of J which is available for redundancy resolution. It can be shown 
that plugging (3.32) in (2.4) yields the same end-effector resolved acceleration 
as in (2.6) [20]. 

The matrix projecting arbitrary joint accelerations into the null space of 
J is given by {I — J), no matter what choice is made for the weighting 

matrix W in (3.33). Therefore, it is significant to choose W so that the 
redundancy resolntion scheme for motion control should not be altered when 
interaction with the environment occurs. To this purpose, from (2.2) the joint 
accelerations induced by the external end-effector force and moment are given 

by 

Qe = -B ^{q)J^{q)h. 

Projecting these accelerations in the null space of the Jacobian gives 



(3.34) 
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= - (/ - jHQ)JiQ)) B-\q)J^{q)h. (3.35) 

Choosing W — B (3.33) and plugging the resulting in (3.35) yields 
"ien — Oj meaning that the external force and moment produce no null space 
joint accelerations. Therefore, in view of this choice, in the design of the joint 
resolved acceleration in (3.32) the vector can be used to solve redundancy 
independently of the occurrenee of interaction with the environment. The 
matrix 

^ (3.36) 

weighted by the inertia matrix is termed dynamieally eonsistent pseudo- 
inverse of the Jacobian. 




Figure 3.1. Spatial impedance control with redundancy resolution 



The next step consists of designing a redundancy resolution eontrol in 
terms of the null space joint accelerations a„ in (3.32). To this purpose, a„ 
shall be ehosen so as to ensure stabilisation of the null space motion and 
possibly optimisation of an additional task function. Let 

e„= (/- J^(q)J(q)) (/3-q) (3.37) 

denote the null space velocity error where /3 is a joint velocity vector which 
is available for redundancy resolution. The goal is to make e„ asymptotieally 
converge to zero. Choosing 

«„ = (/- J^J) (/3 - / J(^ -q)+ B^\Kr,e„ + Ce„)) (3.38) 

where Kn is a positive definite matrix, it can be shown that e„ — > 0 asymp- 
totically [20]. 
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Regarding the utilisation of redundancy, a typical choice for (5 is 

= (3.39) 

where kj 3 is a signed scalar and w{q) is an additional task function that can 
be locally optimised. 

A block diagram summarising the overall spatial impedance control with 
redundancy resolution is sketched in Figure 3.1. 



4 Cooperative Robots 

4.1 Loose Cooperation 

Consider a system of two robots manipulating an object. A cooperative con- 
trol strategy can be termed loose when the manipulation task is executed by 
controlling the two robots in an independent fashion. Cooperation is realised 
only at the task planning level. 

A typical task requiring loose cooperative control is constituted by mating 
rigid parts such as dual-robot assembly in a workcell. The archetype is the 
classical peg-in-hole, where one robot carries the peg and the other holds 
the hollow part. It should be clear that the task is successfully executed 
provided that mating forces are suitably reduced during the insertion so as 
to avoid undesirable jamming and wedging. This concept can be brought to 
fruition by resorting to special mechanical devices such as the Remote Center 
of Compliance in [28] or the compliant end effectors in [11], [15]. 

An alternative strategy [5] is to assign complementary roles to the two 
robots, i.e. to operate one robot using pure positional control while controlling 
the other so as to achieve a programmable impedance at the end effector. 
In detail, the motion of the position controlled robot is planned to match 
the nominal requirements of the assigned task, while the active compliant 
behaviour imposed to the impedance controlled robot is devoted to mitigating 
the effects of imperfect knowledge of the task geometry and unavoidable 
tracking errors. 

The position controlled robot can be operated using the standard indus- 
trial control unit, i.e. by exploiting the set of motion planning instructions of 
the native programming language. 

On the other hand, assuming that an open control architecture is available 
for the other robot, an impedance control is realised at the end-effector level 
where the object is either the peg or the hollow part; the end-effector frame 
of such a robot coincides with the object frame. 
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4.2 Tight Cooperation 

A cooperative control strategy can be termed tight when the manipulation 
task is executed by controlling the robots in a coordinated fashion. Cooper- 
ation is realised not only at the task planning level, but also at the control 
level. This is the typical task of two robots whose end effectors tightly grasp 
a commonly held rigid object, thus creating a closed-kinematic chain. 

Consider a two-robot system tightly grasping a rigid object in contact 
with the environment. Let So be the frame attached to the object; its origin 
and orientation with respect to the base frame are characterised by the (3 x 1) 
position vector and the (3 x 3) rotation matrix Ro, respectively. 

The object motion must be related to the motions of the end effectors of 
the robots. This can be done by resorting to the task-oriented formulation 
for coordinated motion of dual- robot systems developed in [9], [3]. 

For each manipulator (fc = 1, 2) let Sk denote a frame attached to the 
end effector; its origin and orientation with respect to the base frame are 
characterised by the (3x1) position vector pj, and the (3 x 3) rotation 
matrix Rk, respectively. Then, Qk = {rjk,£k} represents the unit quaternion 
corresponding to Rk- Let also Vk — [pj tie the (6 x 1) end-effector 

(linear and angular) velocity vector. 

The object position is chosen as 

Po=\{Pl+P2), (4-1) 

while the rotation matrix defining the orientation of So is chosen as 

Ro = Ri^RCr2i,p2i/2), (4.2) 

where ^V 2 i and '^21 are respectively the unit vector and the angle that realise 
the rotation described by 

^R2 = R^R2 (4.3) 

and ^i?(^r 2 i, '^ 21 / 2 ) is the rotation matrix corresponding to a rotation of 
^ 21/2 about the axis ^r 2 i. Then the absolute orientation can be expressed as 

Qo = Qi * jcos^, sin ^^ 21 1 (4.4) 

where Qo is the unit quaternion corresponding to Ro and denotes the 
quaternion product (see the appendix in [25]); the second factor on the right- 
hand side of (4.4) is the unit quaternion extracted from ^R{^r 21 ,^ 21 / 2 ). 

From (4.1) and (4.2), the object linear velocity and angular velocity uio 
can be expressed as 

Vo = -^{vi +V2) 



where Vo = [pj 



(4.5) 
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Let fj. and [k = 1, 2) respectively denote the (3 x 1) end-effector force 
and moment vectors for either manipulator. Then, according to the kineto- 
statics duality concept [26] applied to (4.5), the object force and moment can 
be expressed as 

ho = hi + ft-2 (4.6) 

where /ifc = [/fc nj and ho = [ 

In order to fully describe a coordinated motion, the position and orienta- 
tion of one manipulator relative to the other is also of concern. The mutual 
position between the two end effectors is the vector 

^P21=P2~Pl- (4-7) 

The mutual orientation between the two end effectors is defined with reference 
to El in terms of the rotation matrix ^i ?27 and then in terms of the quaternion 
product 

Q21 — Qi^ * Q2, (4-8) 

where Qi^ = {rji, — ei} is the unit quaternion corresponding to (i.e. the 
conjugate of Qi). 

From (4.3) and (4.7), the mutual velocity can be expressed as 



Av21=V2-Vi. (4.9) 

Once the position and orientation variables describing the cooperative 
system have been defined, reference end-effector position p^j. and orienta- 
tion Qr^k variables, as well as reference end-effector velocities Vr^k {k = 1,2), 
must be generated with a twofold objective; namely, realising an impedance 
behaviour at the object level, while assigning a mutual position and ori- 
entation between the two end effectors that is compatible with the object 
geometry. 

The first objective can be fulfilled as follows. Let the desired object posi- 
tion and orientation Qd (extracted from the desired rotation matrix Rd) 
be assigned with the associated linear and angular velocities and accelera- 
tions. Also, the object force and moment can be computed from (4.6) with 
the end-effector forces and moments available from the wrist force/torque 
sensors. Then, the translational and rotational impedance equations (3.1) 
and (3.30) are integrated, with input and to compute Po and '^chc, Po 
and '^aic, and then p^ and Qc via the quaternion propagation. 

The second objective can be fulfilled by assigning a reference mutual po- 
sition Ap^ 21 s-nd orientation Qr, 21- In particular, Ap^. 21 and Qr, 21 are taken 
as constant and equal to the initial values of Ap 2 i in (4.7) and Q 21 extracted 
from (4.3), respectively, that can be computed via the direct kinematics of 
the two manipulators. 

The two objectives are combined by choosing the above reference position 
and orientation for the two end effectors so as to satisfy (4.1), (4.7), and (4.4), 
(4.8), i.e. 




F. Caccavale, C. Natale, B. Siciliano, L. Villani 
1 ^ 

PrA = Pc - 2 ^Pr ,21 


(4.10) 


1 ^ 

Pr,2 = Pc + 2 ^Pr ,21 


(4.11) 


r\ i '^r,21 . "dr, 21 \ \ 

Qr,l = Qc * S COS , - sm r^,21 I 


(4.12) 


Sr, 2 = Sr,l * Sr,21- 


(4.13) 


Purther, the reference velocities for the two end effectors 


are chosen as 


1 ^ 

Vr,l = Vc - -2^Vr,21 


(4.14) 


1 ^ 

Vr,2 = Vc+ -2^Vr,21 


(4.15) 



where Vc = [pj Then, the reference accelerations can be computed 

via a formal time derivative of the terms in (4.14) and (4.15). 

The above reference trajectories can be tracked by resorting to an an inner 
motion control loop based on an inverse dynamics strategy as for the case of 
the single manipulator. 



5 Experimental Validation 

The setup in the PRISMA Lab consists of two industrial robots Comau 
SMART-3 S (Figure 5.1). Each robot arm has a six-revolute-joint anthropo- 
morphic geometry with nonnull shoulder and elbow offsets and non-spherical 
wrist. One arm is mounted on a sliding track which provides an additional de- 
gree of mobility. The joints are actuated by brushless motors via gear trains; 
shaft absolute resolvers provide motor position measurements. Each robot is 
controlled by the C3G 9000 control unit which has a VME-based architec- 
ture. Independent joint control is adopted where the individual servos are 
implemented as standard PID controllers. The native robot programming 
language is PDL 2, a high-level Pascal-like language with typical motion 
planning instructions. 

An open version of the control unit is available [2] which allows testing of 
advanced control algorithms on a conventional industrial robot. Connection 
of the VME bus of the C3G 9000 unit to the ISA bus of a standard PC is 
made possible by a BIT 3 Computer bus adapter board; for the experiments, 
a PC Pentium MMX/233 was used. 

Various operating modes are available in the control unit, allowing the 
PC to interact with the original controller both at trajectory generation level 
and at joint control level. To implement model-based control schemes, the 
operating mode number 4 is used in which the PC is in charge of comput- 
ing the control algorithm and passing the references to the current servos 
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Figure 5.1. Experimental setup available in the PRISMA Lab 



through the communication link at 1 ms sampling time. Joint velocities are 
reconstructed through numerical differentiation of joint position readings. 

A six-axis force/torque sensor ATI FT30-100 with force range of ±130 
N and torque range of ±10 N-m can be mounted at either arm’s wrist. The 
sensor is connected to the PC by a parallel interface board which provides 
readings of six components of generalised force at 1 ms. 

5.1 Single Manipulator 

The above impedance control schemes have been tested in a number of ex- 
periments. An end effector has been built as a steel stick with a wooden disk 
of 5.5 cm radius at the tip. The end-effector frame has its origin at the center 
of the disk and its approach axis normal to the disk surface and pointing 
outwards. 

First case study: Interaction with environment. The first case study has been 
developed to analyse interaction with environment. This is constituted by a 
flat plexiglas surface. The translational stiffness at the contact between the 
end effector and the surface is of the order of 10"^ N/m, while the rotational 
stiffness for small angles is of the order of 20 Nm/rad. 

The task consists of taking the disk in contact with the surface at an 
angle of unknown magnitude (Figure 5.2). The end-effector desired position 
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is required to make a straight-line motion with a vertical displacement of 
—0.24 m along the Zf,-axis of the base frame. The trajectory along the path 
is generated according to a fifth-order interpolating polynomial with null 
initial and final velocities and accelerations, and a duration of 7 s. The end- 
effector desired orientation is required to remain constant during the task. 
The surface is placed (horizontally) in the XhYf,-plane in such a way as to 
obstruct the desired end-effector motion, both for the translational part and 
for the rotational part. 




Figure 5.2. End effector in contact with plexiglas surface 



The parameters of the translational part of the six-DOF impedance equa- 
tion (3.1) have been set to Mp — 91, Dp — 2000/, Kp — 700/, while the 
parameters of the rotational part of the six-DOF impedance equation (3.30) 
have been set to Mo — 0.4/, Do — 5/, Ko — 21. Notice that the stiffness 
matrices have been chosen so as to ensure a compliant behaviour at the end 
effector (limited values of contact force and moment) during the constrained 
motion, while the damping matrices have been chosen so as to guarantee a 
well-damped behaviour. 

The gains of the inner motion control loop actions in (2. 9), (2. 15) have 
been set to Kpp = 2025/, Kpo = 4500/, K pp = K po = 65/. 

The results in Figure 5.3 show the effectiveness of the quaternion-based 
six-DOF impedance control. After the contact, the component of the posi- 
tion error between Ed and E^ 2^Pde = Pd~Pe along the Zt-axis significantly 
deviates from zero, as expected, while small errors can be seen also for the 
components along the Xi,- and the Ifc-axis due to contact friction. As for 
the orientation error, all the components of the orientation displacement be- 
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tween Ed and Ee ('^Cde) significantly deviate from zero since the end-effector 
frame has to rotate with respect to the base frame after the contact in order 
to comply with the surface. Also, in view of the imposed task, a prevailing 
component of the contact force can be observed along the Z{,-axis after the 
contact, while the small components along the Xi,- and the Fb-axis arise as a 
consequence of the above end-effector deviation. As for the contact moment 
referred to Ed, the component about the Zt-axis is small, as expected. It 
can be recognised that all the above quantities reach constant steady-state 
values after the desired motion is stopped. The oscillations on the force and 
moment during the transient can be mainly ascribed to slipping of the disk 
on the surface after the contact. 



position error 




contact force 





Is] 



contact moment 




Figure 5.3. Experimental results under six-DOF impedance control based on 
quaternion in the first case study 



In sum, it can be asserted that a compliant behaviour is successfully 
achieved. A similar performance has been obtained also with the six-DOF 
impedance control schemes based on the Euler angles error, i.e. by using 
either (3.3) or (3.6) in lieu of (3.30). This fact can be explained because 
both the absolute end-effector orientation in (3.3) and the relative orientation 
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in (3.6) keep far from representation singularities. The results are not reported 
here for brevity. 

Second case study: Representation singularity. The second case study is 
aimed at testing the performance of the quaternion-based compared to the 
Euler angles-based six-DOF impedance control, when the end-effector orien- 
tation is close to a representation singularity of T. The end effector and the 
surface are the same as in the previous case study. 

The end-effector desired position is required to make a straight-line mo- 
tion with a horizontal displacement of 0.085 m along the X^-axis of the base 
frame. The trajectory along the path is generated according to a fifth-order 
interpolating polynomial with null initial and final velocities and accelera- 
tions, and a duration of 5 s. The end-effector desired orientation is required 
to remain constant during the task. The surface is now placed vertically in 
such a way as to obstruct the desired end-effector motion, only for the rota- 
tional part though. Therefore, no impedance control has been accomplished 
for the translational part, i.e. in (2.9) coincides with p^. 

The parameters of the quaternion-based impedance equations (3.1), (3. 30) 
are set to Mp = 10/, Dp — 600/, Kp = 1000/, Mg — 0.25/, Do — 3.5/, 
Ko = 2.51. In order to carry out a comparison, the impedance control based 
on the Euler angles has also been tested. The parameters of the rotational 
impedance equation (3.3) have been set to the same values as for the quater- 
nion. As regards the gains of the inner motion control loop, these have been 
chosen equal to those in the previous experiment for both types of impedance 
control schemes. 



contact force contact moment 





Figure 5.4. Experimental results under six-DOF impedance control based on 
quaternion in the second case study 



The results in Figures 5.4 and 5.5 show the significant differences occur- 
ring in the performance of the two schemes. For the impedance control based 
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Figure 5.5. Experimental results under six-DOF impedance control based on the 
classical Euler angles in the second case study 



on (3.3), large values of contact force and moment are generated since the ro- 
tational impedance equation suffers from ill-conditioning of the matrix 
this phenomenon is not present for the quaternion-based impedance control 
based on (3.30) since representation singularities are not involved in the ro- 
tational impedance equation. On the other hand, testing of the impedance 
control based on the alternative Euler angles in (3.6) has revealed a perfor- 
mance as good as the quaternion-based impedance control, since the orien- 
tation displacement is kept far from a representation singularity. Hence, 
the results are not reported here for brevity. 

In sum it can be concluded that both the impedance control based on the 
alternative Euler angles and the quaternion-based impedance control perform 
better than the impedance control based on the classical Euler angles, as far 
as interaction with the environment is concerned. 

Third case study: Task geometric consistency. Another case study has been 
developed to analyse task geometric consistency when an external moment 
is applied at the end effector. The quaternion-based impedance control and 
the impedance control based on Euler angles have been tested. 

The stiffness matrices of the rotational impedance equations (3.30), (3.3) 
have been taken as diagonal matrices; Kg = U^lrgUo has been chosen with 
Uo = I and To = 2.5/ for both schemes. The remaining parameters of the 
rotational impedance have been set to Mg = 0.25/ and Dg = 1.5/ for both 
schemes. No impedance control has been accomplished for the translational 
part. The gains of the inner motion control loop have been chosen equal to 
those in the previous case study. 

The position and orientation of the desired frame are required to remain 
constant, and a torque is applied about the approach axis of Ed; the torque 
is taken from zero to 2.5 Nm according to a linear interpolating polynomial 
with 4th-order blends and a total duration of 1 s. 






144 F. Caccavale, C. Natale, B. Siciliano, L. Villani 




0 2 4 6 8 10 

[s] 




[s] 



Figure 5.6. Experimental results under six-DOF impedance control based on 
quaternion in the third case study 
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Figure 5.7. Experimental results under six-DOF impedance control based on clas- 
sical Euler angles in the third case study 



The results in Figures 5.6 and 5.7 show the different performance in terms 
of the orientation misalignment 6 which has been defined as the norm of the 
vector product between the orientation error and the nnit vector Uqs of the 
approach axis of Ed, i.e. 

^ = ||S'(^edeK3||. 

For the impedance control based on (3.3) the instantaneous axis of rotation 
of Eg changes, while remarkably no misalignment occurs for the impedance 
control based on (3.30). The impedance control based on (3.6) has also been 
tested and its performance is as good as that of the quaternion-based control; 
hence, the results are not reported for brevity. 

Fourth case study: Nondiagonal rotational stiffness. In the fourth case study, 
the quaternion-based impedance control and the impedance control based on 
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the alternative Euler angles have been tested when a nondiagonal rotational 
stiffness is chosen. The impedance control based on the classical Euler angles 
has been ruled out in view of the poor results of the previous experiment. 

The principal axes of the stiffness matrices of the rotational impedance 
equations (3. 30), (3. 6) are rotated with respect to the coordinate axes of Sd', 
Ko = UjroUo has been chosen with 
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for both schemes. The remaining parameters of the rotational impedance 
have been set to Mo = 0.25/ and Do = 1.5/ for both schemes. As above, 
no impedance control has been accomplished for the translational part, and 
the gains of the inner motion control loop have been chosen equal to those 
in the previous case study. A torque has been applied about the axis whose 
unit vector is Mqs; the torque is taken from zero to —1.5 Nm according to a 
linear interpolating polynomial with 4th-order blends and a total duration of 
1 s. 
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Figure 5.8. Experimental results under six-DOF impedance control based on 
quaternion in the fourth case study 



The results in Figures 5.8 and 5.9 show the significant differences oc- 
curring in terms of the orientation misalignment 6. It can be seen that the 
instantaneous axis of rotation of does not appreciably rotate with the 
impedance control based on (3.30), given the performance of the inner loop 
acting on the end-effector orientation error. Instead, a significant misalign- 
ment occurs with the impedance control based on (3.6). 

In sum, it can be concluded that the quaternion-based impedance control 
performs better than both impedance control schemes based on the Euler 
angles as far as task geometric consistency is concerned. 
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Figure 5.9. Experimental results under six-DOF impedance control based on al- 
ternative Euler angles in the fourth case study 



Fifth case study: Redundancy resolution. The six-DOF quaternion-based 
impedance control has been tested in a case study when a redundancy reso- 
lution scheme is incorporated into the motion control. 

The environment is constituted by a cardboard box. The translational 
stiffness at the contact between the end effector and the surface is of the 
order of 5000 N/m, while the rotational stiffness for small angles is of the 
order of 15 Nm/rad. 

The task in the experiment consists of four phases; namely, reconfiguring 
the manipulator, approaching the surface, staying in contact, and leaving the 
surface. To begin, the additional task function in (3.39) has been chosen as 

w{q) = ^(93 - qzdf 

where qz is the elbow joint and gad is a desired trajectory from the initial value 
of ga to the final value of 1.1 rad in a time of 4 s with a fifth-order interpolating 
polynomial with null initial and final velocity and acceleration. This function 
is aimed at reconfiguring the manipulator in a more dexterous posture before 
contacting the surface. After a lapse of 4 s, the disk is taken in contact with 
the surface at an angle of 77 t/ 36 rad. The end-effector desired position is 
required to make a straight-line motion with a horizontal displacement of 
0.08 m along the Zi, axis of the base frame. The trajectory along the path is 
generated according to a fifth-order interpolating polynomial with null initial 
and final velocities and accelerations, and a duration of 2 s. The end-effector 
desired orientation is required to remain constant during the task. The surface 
is placed (vertically) in the AbTb-plane of the base frame in such a way to 
obstruct the desired end-effector motion, both for the translational part and 
the rotational part. After a lapse of 13 s in contact, the end-effector motion 
is commanded back to the initial position with a duration of 4 s. 
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Figure 5.10. Experimental results under six-DOF impedance control based on 
quaternion with redundancy resolution in the fifth case study 
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Figure 5.11. Additional task function in the hfth case study 
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The parameters of the translational impedance (3.1) have been set to 
Mp = 16/, Dp = diag{800, 800, 250} and Kp = diag{1300, 1300, 800}, while 
the parameters of the rotational impedance (3.30) have been set to Mo — 
0.7/ and Do = 4/, Ko = 2.5/. 

The gains of the inner motion control loop in (2. 9), (2. 15) have been set to 
Kpp = 2250/ and Kpo = 4000/, Kp>p = 70/ and Kp>o = 75/. The gains of 
the redundancy resolution control in (3. 38), (3. 39) have been set to Kn — 20/ 
and kp = 250. 

The results in Figure 5.10 show the effectiveness of the six-DOF impedance 
control with redundancy resolution. During the reconfiguration (8 s), the 
components of the position error — Pd~Pe between Ed and Eg and of 
the orientation error between Ed and Eg are practically zero, meaning 
that the dynamics of the null space motion does not disturb the end-effector 
motion. Such error remains small during the approach (2 s). During the con- 
tact (13 s), the component of the position error along the Zf,-axis significantly 
deviates from zero, as expected; as for the orientation error, the component 
of the orientation error along the Fg-axis significantly deviates from zero 
since Ee has to rotate about Fg in order to comply with the surface. Also, 
in view of the imposed task, a prevailing component of the contact force can 
be observed along the ^h-axis after the contact, whereas the sole component 
of the contact moment about the F}-axis is significant, as expected. During 
the takeoff (4 s), both the errors and the contact force and moment return 
to zero. 

The same task has been executed again for the impedance control with- 
out redundancy resolution (kp = 0). The performance in terms of the contact 
between the end effector and the surface is the same as above since the ad- 
ditional task does not interfere with the primary interaction task; hence, the 
time history of the relevant quantities is omitted for brevity. Nevertheless, a 
comparison between the two cases in Figure 5.11 shows that the task func- 
tion is successfully optimised when redundancy is exploited (solid) other than 
when redundancy is not exploited (dashed). 

5.2 Loose Cooperation 

A typical assembly task has been performed to test the effectiveness of the 
quaternion-based impedance control strategy for execution of interaction 
tasks where the geometry of the contact plays a crucial role. The task is 
the classical peg-in-hole, which can be executed by a dual-arm system if one 
robot carries the peg and the other holds the hollow part. 

The task for the seven-joint manipulator is planned as follows. From a 
given posture a joint space motion is commanded to reach a suitable inter- 
mediate posture which facilitates the subsequent phases of the task; then, 
a Cartesian space motion along a straight-line path is commanded to drive 
the tip of the peg in the proximity of the mouth of the hole and align the 
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approach axis of the peg with the axis of the hole, as accurately as possi- 
ble. Finally, a straight-line motion along the approach axis — typically at a 
reduced speed with respect to the previous phase — is commanded to realise 
the insertion. The described task is specihed in the following PDL 2 program: 

PROGRAM insertion CONST spd=3 
dpt=50 

VAR int: JOINTPOS 
pro: POSITION 

BEGIN 

— move joints to intermediate posture 
MOVE TO int 

— move tip to proximate pose 
MOVE LINEAR TO pro 
— set arm speed override at spd °L 
$ARM_0VR := spd 

— move tip along approach, axis by dpt mm 
MOVE RELATIVE VEC(0,0,dpt) IN TOOL 

END insertion 

where the joint position int and the end-effector pose pro are taught before- 
hand, while the constants spd and dpt are set according to task requirements. 

The six-joint manipulator is controlled using the open operating mode so 
that the end effector behaves as the programmed six-DOF impedance. To 
this purpose, the force/torque sensor is mounted at the wrist. 

For the task at issue, the position and orientation of the desired frame is 
taken as a constant, i.e. the six-joint manipulator is controlled to stay still. 
Whenever a contact force and/or moment is experienced at the end effector, 
this reacts according to the programmed impedance where the origin of the 
desired frame determines the location of the Remote Center of Compliance. 

The peg is a wooden cylinder of 17 mm diameter and 80 mm height, while 
the hollow part is a wooden block with a hole of 18 mm diameter and 70 mm 
depth; that is, a 0.5 mm radial tolerance is present during the insertion. 

The insertion task is programmed in terms of a planned motion for the 
seven-joint manipulator described by spd = 3 and dpt = 50. 

The six-joint manipulator is impedance-controlled so that the Remote 
Center of Compliance is located at = Pg — [0 165 O]"’" mm since 
the insertion direction is along the FZ-axis of the base frame. Also, it is 
Rd = Ro- The parameters of the impedance equation in (3.1), (3. 30) are re- 
spectively set to Mp = diag{15, 40, 15}, Dp = diag{300, 950, 300}, Kp = 
diag{400, 1300, 400}, for the translational part, and Mo9I, Do — 13.5/, 
Ko = /, for the rotational part. 

Since the experiment is aimed at testing the robustness of the proposed 
strategy in the case of incorrect task planning, an additional misalignment 
has been intentionally introduced by rotating the end-effector frame of the 
seven-joint manipulator by 2 deg about the Af,- and the Zb-axis of its base 
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frame. Further, the length of the path along the approach axis of the peg 
has been set to dpt = 90, corresponding to a 20 mm overshoot beyond the 
bottom of the hole. The same impedance parameters as above are chosen. 



mating force mating moment 





Figure 5.12. Force and moment in the peg-in-hole task 



The results are illustrated in Figure 5.12 in terms of the time history 
of the three components of mating force and moment. Interestingly enough, 
the values of force and moment keep limited despite of the incorrect task 
planning; moreover at steady state, nonnull values of force and moment can 
be observed which are obviously caused by the planned misalignment and 
overshoot. 

5.3 Tight Cooperation 

In the experiment devoted to testing the proposed tight cooperative control 
strategy, the two robot end effectors tightly grasp the ends of a wooden bar 
of 1 m length. At the center of the bar is fixed a steel stick with a wooden 
disk of 5.5 cm radius at its tip. 

The environment is constituted by a cardboard box; the translational 
stiffness at the contact between the disk and the surface is of the order of 
5000 N/m, while the rotational stiffness for small angles is of the order of 
15 Nm/rad. 

The task in the experiment consists in taking the disk in contact with 
the surface; that is placed at an unknown distance with an angle of unknown 
magnitude. The origin of So is required to make a desired motion along a 
straight line with a vertical displacement of —0.275 m along the Zf,-axis of Sb. 
The trajectory along the path is generated according to a 5th-order interpo- 
lating polynomial with null initial and final velocities and accelerations, and 
a duration of 6 s. The desired orientation of the object frame is required to 
remain constant. 
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Figure 5.13. Experiment of tight cooperative control: Left: Object position and 
orientation displacement between Ed and Eo- Right: Contact force and moment 
acting on the object 




The parameters of the translational part of the impedance equation (3.1) 
have been set to Mp = 30/, Dp = 555/, Kp = 1300/, while the parameters 
of the rotational part of the impedance equation (3.30) have been set to 
Mo = diagjlO, 2, 10}, Do = diag{35, 20, 35}, Kg — diag{20, 8, 20}. Notice 
that the stiffness matrices have been chosen so as to ensure a compliant 
behaviour (limited values of contact force and moment) during the contact, 
while the damping matrices have been chosen so as to guarantee a well- 
damped behaviour. The feedback gains in (2.9, (2.15) have been set to kup = 
65, kpp — 1800 for the position loop, and kpo — 65, kpo = 3600 for the 
orientation loop, respectively. 

From Figure 5.13 {left), after the contact, the component along the Zt~ 
axis of the position displacement between the desired frame Ed and the object 
frame Eg, expressed in Ei,, significantly deviates from zero, as expected; a 
smaller displacement can also be seen for the component along the Xf,-axis, 
due to contact friction. As for the orientation displacement between Eg and 
Ed, expressed in Ed, only the component along the F^-axis significantly de- 
viates from zero since the object frame has to rotate about the F^-axis of Ed 
in order to comply with the surface after the contact. 
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From Figure 5.13 (right), in view of the imposed task, a prevailing compo- 
nent of the contact force can be observed along the Zh-axis after the contact, 
while a significant component along the X^-axis arises, corresponding to the 
above position displacement. As for the contact moment, the only nonneg- 
ligible component is that along the Fd-axis of Sd, which corresponds to the 
above orientation displacement. It can be recognised that all the above quan- 
tities reach constant steady-state values after the desired motion is stopped. 
The oscillations on the force and moment can be ascribed to the flexibility 
effects of the commonly held object. 



6 Conclusion and Future Directions 

An impedance control strategy has been presented for six-DOF tasks. By 
considering an angle/axis representation of the end-effector orientation dis- 
placement, a six-DOF impedance equation has been derived, which exhibits 
a physically meaningful behavior and task geometric consistency properties. 
Among the different angle/axis representations, the unit quaternion has been 
chosen which avoids the occurrence of representation singularities. The supe- 
rior performance of the proposed angle/axis-based impedance controller over 
two different impedance controllers based on Euler angles has been shown 
both in theory and in practice. 

For the case of redundant manipulators, the dynamically consistent pseu- 
doinverse of the manipulator Jacobian has been adopted to decouple the 
dynamics of the end-effector motion from the null-space motion. Moreover, 
redundancy is exploited to stabilise null-space joint velocities and optimise 
an additional task function. 

Finally, the problem of two cooperative robots manipulating a common 
object has been considered and both loose and tight cooperation have been 
addressed. In the case of loose cooperation, limited interaction forces have 
been achieved by adopting the geometrically consistent impedance control 
strategy for one of the two manipulators. In the case of tight cooperation, 
interaction of the grasped object with the environment has been managed by 
enforcing a geometrically consistent impedance behaviour at the object level. 

The proposed control strategies have been experimentally tested on a 
set-up composed by two industrial robots Comau SMART-3S with two 
force/torque sensors ATI FT-30/100 mounted at the wrist. 

Future work will be devoted to extending the use of geometrically con- 
sistent impedance for cooperative manipulators. Namely, the adoption of an 
impedance control strategy for controlling the internal forces at the grasped 
object is currently under investigation. Under this regard, an open problem is 
the cooperative manipulation of flexible objects. It is expected that the adop- 
tion of impedance control may provide a simple and robust way to cope with 
manipulation of flexible objects or manipulation of rigid objects via elastic 
grasp. 
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Promising future research will be directed towards the integration of the 
interaction control strategies, based on the use of position and force/torque 
sensors, with information coming from vision sensors. Indeed, the adoption of 
sensory feedback coming from cameras is expected to enhance the robustness 
of the control loop as well as may represent a decisive step towards robotic 
systems capable of autonomous interaction with the surrounding environ- 
ment, even partially. 

Last but not least, it should be mentioned that the problem of ensuring 
a safe interaction between the robot the environment can be considered as a 
major objective to be pursued in the next few years. In the case of interaction 
with an artificial environment, suitable strategies must be adopted aimed 
at preventing and/or managing robot failures and abrupt changes of the 
environment conditions, so as to avoid damage for the various devices involved 
in the contact and ensure task completion. On the other hand, when the 
human/robot interaction is of concern (e.g. in anthropic and service robotics), 
safety of the human subject becomes the primary objective to be achieved, 
even at the expense of the task assigned to the robot. 
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Impact modelling and control can be fundamental in robotics, for the execu- 
tion of tasks that require the interaction of the robot with an external body, 
ensuring that a stable contact is achieved without rebounds. This chapter 
examines various issues related to impact modelling and control, devoting a 
particular attention to the main results of the last decade. After the illustra- 
tion of some general approaches to impact modelling in robotics applications, 
various impact control schemes are surveyed, highlighting the possible prob- 
lems that can limit in practice their performances. For this reason, particu- 
lar attention is devoted to those control schemes whose performances have 
been also experimentally tested. A general classification between switching 
and non-switching impact control schemes is introduced, and some control 
solutions, developed by the author following a non-switching approach, are 
illustrated and discussed. 



1 Introduction 

The most recent robotics applications are not restricted to the classical fields, 
such as the industrial and the manufacturing ones, but they are relative to 
various contexts, e.g. as biomedical services, operations in hostile environ- 
ments, and entertainment, too. 

The most common control schemes, which are widely used in the indus- 
trial field, are only partially suitable for such new applications, as they are 
mainly constituted by position control schemes, while several among these 
new tasks require the interaction of the robot with some external object. 
During the last years, many research studies about robot interaction control 
have been developed: some of the most recent and important results are pre- 
sented and discussed in a devoted chapter of this monograph. But several 
works about force and impedance control (see the numerous references in the 
previous chapter) deal with the control of robot interaction with an external 
environment, only once a stable contact situation has been already, somehow, 
achieved. In practice, this can be difficult to be obtained with no rebounds 
without an explicit control of the transition phase of the robot, from the free 
motion condition to the constrained one. Impact control can then be fun- 
damental not only to preserve the mechanical structure of the robot from 
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failures, which can be generated when impact forces are not properly recog- 
nised and taken under control, but also as a control goal, for the execution of 
tasks in which the robot transition toward a contact situation (with an ex- 
ternal object or with another cooperating robot) must be performed without 
rebouncing and loss of contact. 

The first matter to be analysed is the construction of a suitable impact 
model for control. The basic physical phenomena that occur when two or 
more bodies collide have been widely studied and analysed in several books, 
such as e.g. [1], [7], [17], [30]. In the development of the dynamic model of 
a robot striking an external object, it is fundamental to properly choose a 
satisfying compromise between a too detailed description of impact from a 
mechanical and structural point of view (to avoid too complex equations) , and 
an over-simplihed model, which cannot adequately explain those instability 
and bouncing problems that may occur in practice, and that must be afforded 
in the design of the control scheme. Interesting works appeared in the last 
decade about impact modelling in robotics (see e.g. [1], [9], [10], [22], [25], 
[29]), as it will be discussed in Section 2. 

No general theory is available for impact control, because the dynamic 
equations, which can adequately describe the impact of a robot with an ex- 
ternal environment, strongly depend on the geometry of both the manipulator 
and the environment, and on their physical and structural properties, which 
may be unknown or only partially known. Due to the sudden change of the 
equations of motion, that occurs when the bodies involved in the impact 
switch sharply from a condition of non-contact to a condition of contact, the 
presence of some unexpected disturbances can make the manipulator leave 
the contact surface after impact, and limit cycles or instabilities may be ex- 
cited. Besides, quite powerful tools are required for the practical application 
of many impact control schemes, e.g. for precise data storing and/or for the 
impact detection: experimental tests are then fundamental to validate the 
actual effectiveness and performances of the developed impact control strate- 
gies. 

A survey of the most recent results obtained for impact control in the 
robotic field is presented in Section 3, by introducing a general classification 
of them with reference to the existence or not of a “switching condition” in the 
definition of the control scheme, i.e. by distinguishing if a unique control law is 
applied during free-motion, contact mode and in the impact transition phase 
between them, or if different controllers are employed in the three situations, 
by switching from a control law to another one. A particular attention is 
devoted to the employment of integral terms in the switching control laws, as 
such terms, which are crucial for force control in the contact condition, can 
determine bouncing and instability during the transition phase. 

In Section 4 some impact control solutions, which have been developed by 
the author together with Tornambe (see [12]-[16]), with the aim of studying 
the possibility to achieve satisfying results by the application of a unique 
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control law (both in free motion and during contact), are illustrated and 
discussed on the basis of some experimental results, too. 

Section 5 draws some final conclusions. 



2 Impact Modelling 

A good impact model of a robot, striking some external environment, must 
be the result of a satisfying compromise: in fact, it must be quite simple, 
to be suitable for the subsequent controller design, but at the same time 
it must correctly take into account the various phenomena involved in the 
impact itself. Some more or less restrictive assumptions must then be made 
to a priori establish the approximation level to be used to describe impact. 
In particular, assumptions must be made about the possibility of neglecting 
or not plastic deformations of the structures of the involved systems, about 
the presence or not of friction between the impacting surfaces, and about 
the importance of rotational effects, in order to simplify the model, which 
must correctly describe how the different parameters (such as e.g. masses, 
geometric dimensions, stiffness, impact duration) affect the collision. 

The basic characteristic of the models of impacting systems is given by the 
existence of two separate conditions, the free motion one and the contact one, 
together with a transition phase between them (i.e. the actual impact phase). 
Kinematic and/or dynamic constraints may hold in the contact condition and 
during the transition phase, which is characterised by a jump in the velocities 
of the systems involved in the impact: the relation between post- and pre- 
impact velocities is specified by the so-called restitution rule [17], [30]. 

Some of the main results that appeared in literature in the last decade 
about impact modelling in the robotic field are briefly recalled and discussed 
hereafter. 

2.1 Some “Pioneering” Works 

The restitution rule, relating post- and pre-impact velocities of colliding sys- 
tems, is the basis of the impact model that was developed and experimentally 
validated in the paper [29], which is to be mentioned among the ones that 
appeared in the first 90’s about impact modelling in the robotic field. Here, a 
simple energy approach is used to determine a quite accurate impact model 
in the case of a single-link manipulator (actuated by a high torque DC servo 
motor, geared down by a speed reducer), whose tool comes into contact with 
a workpiece. The model, which takes into account the stiffness of the tool and 
the workpiece, but also the torsional stiffness of the transmission, is based on 
the assumption that when the tool strikes the workpiece for the first time, 
a common impulsive impact force is applied to them for a short time, dur- 
ing which they are still free from each other. Afterwards, when contact is 
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established, the tool and the workpiece can be modelled as a unique lumped 
mass. The impact description is then obtained by dehning the impulse as 
the time integral of the force, and by imposing that it equals the change in 
momentum, expressed as a function of the coefficient of restitution e, dehned 
as: 



g ^ V2J - VlJ 
Vl,i - V2,i ’ 



( 2 . 1 ) 



where Vk,i and Vkj, fc = 1,2, are respectively the initial and hnal velocity 
components of the colliding object fc, in the direction normal to the contact 
surfaces. 

The papers [9] and [10], which appeared too in the first 90’s, deal with 
rigid bodies collisions of planar kinematic chains, as in the case of walking 
robots and locomotion systems, which are subject to periodic impacts with 
the walking surface. 

In particular, the work in [9] deals with the class of kinematic chains 
having one end resting on the ground surface prior to impact, while the other 
end contacts the surface (just as in the locomotion case); a methodology 
is proposed, and applied to some examples, to exactly determine the post 
impact velocities of the kinematic chains, when they strike external surfaces 
in presence of friction. 

Paper [10] is devoted to the solution of multicontact collisions of kinematic 
chains. Two procedures are developed to solve the problem: the first one, 
which is based on a differential formulation of the impact equations, allows 
the computation of the post-impact velocities accordingly to three different 
possible definitions of the coefficient of restitution (the kinematic, the kinetic, 
and the energy one); the second procedure uses the kinematic definition of 
the coefficient of restitution only, on the basis of an algebraie formulation 
of the impact equations (see [10] for details). This last formulation cannot 
be applied to the cases in which rebounds occur at the interaction with the 
surface. The application of the different procedures to a specific example, 
given by a planar three-link chain with two contact points, is used to discuss 
the consistency of the obtained results using the various approaches, and the 
possibility to predict rebounds at the non-impacting contact points. 

A common weak point of these “old” works is represented by the fact that 
they deal with particular cases or classes of systems, so that their results can 
be only partially extended directly to more general applications. 



2.2 General Approaches to Impact Modelling 

A formal, well stated discussion about modelling (and control) of impacting 
systems can be found in [2] (and in a more complete way in the book [1]), 
where a general formulation is proposed for the model of rigid manipulators 
subject to impacts. In particular, the three phases (free motion, contact, and 
transition between them) are defined as recalled hereafter. 
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Let g G 3?" be the vector of the generalised coordinates of an n-DOF rigid 
robot, and let F{q) =0 be the constraint equation, describing a surface with 
which the manipulator must interact. In the most general case, F{q) G 
with m > 1, i.e. multi-dimensional constraints can be considered; in the 
following developments, a codimension-one constraint will be assumed for the 
sake of simplicity. The well-known robot dynamic equations in the free-motion 
phase and in the constrained-motion one can then be written as follows: 
Free-motion phase 



M{q)q-\-C{q,q)q-\- g{q) 


= U 


(2.2) 


n<i) 


> 0. 


(2.3) 


Constrained-motion phase 






M {q)q -\-C{q,q)q-\- g{q) = U 


OF 


(2.4) 


O 

II 


and A > 0. 


(2.5) 



where M{q) G 3?"^" is the positive definite inertia matrix, C{q,q)q contains 
the Coriolis and centrifugal terms, g{q) G 3?" is the vector of the generalised 
gravity torques, C/ G 3?” is the control input vector (to be defined accordingly 
to the required task), and A is the Lagrange multiplier. 

In the first case the robot motion is free from obstacles, and it is described 
by the set of ordinary differential equations given in (2.2). In the classical 
interaction and hybrid force/position control schemes, the robot is generally 
supposed to be already in the constrained-motion condition described by 

(2.4) -(2.5); the constraint F{q) = 0 is assumed to hold for all times, without 
any consideration for possible transitions between robot configurations such 
that F{q) > 0 and other ones such that F{q) = 0. In this way, the system 
trajectories can still be considered as smooth time functions. 

To properly consider impact, the robot dynamic model given by (2.2)- 

(2.5) must be completed with the description of the transition phase between 
the free-motion condition and the constrained-motion one. Let t = Fhe the 
time instant at which a collision occurs; it follows that: 

— F[q{t)] > 0 for t G [tc — 6, tc], for some ^ > 0 small enough, and F[q{tc)] = 
0 ; 

— the velocity q{t) has a discontinuity at t = F, such that 

< 0 and q{tt)'^'VqF[q{tc)] > 0, 

where q{t~) and q{t'^) are, respectively, the left- and the right-limit of the 
velocity q{t) for t F, and VqF[g(tc)] is the gradient of F{q) at t = F] 
the jump in q, denoted as crq{tc), can be specified through the restitution 
rule relating post- and pre-impact velocities; 
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— due to the discontinuity in the velocity, a percussion Pq{tc) acts on the 



system at t = tc, i.e. a generalised impulsive force of the form 


Pq{tc)6tci 


where is the Dirac measure at E- 






The robot dynamic model (2.2)-(2.5) is then 
equations. 


completed by the 


following 


Transition phase 






M{q)q + C{q,q)q + g{q) = 


U 


(2.6) 


F{q) > 


0 


(2.7) 


M[q{tc)]crq{tc) = 


Pq(tc) 


(2.8) 


F[q{tc)] = 


0 


(2.9) 


q{tt) = 


-eqiffi), 


(2.10) 



where e G [0, 1] is the restitution coefficient, characterising the well-known 
Newton’s rule (2.10), which can be extended to higher-dimensional cases by 
a generalised normal rule of the form: 

Qittf'^qFkitc)] = -eq(t-fVqF[q{tc)]. 



It must be stressed that the crucial problem in the impact models, which 
must be suitable for simulation and control design, is the handling of the 
inequality constraints on the generalised coordinates q{t), due to the presence 
of physical obstacles in the robot motion. 

A recent study [25] has shown how the equations of motion, resulting from 
the application of the method of the Valentine variables, constitute a suitable 
way to describe the impact between very stiff mechanical systems (i.e. in case 
of “nonsmooth” impacts), whereas the method of the penalty functions is 
suitable to describe “smooth” impacts, i.e. impacts of mechanical parts that 
show some flexibility. 

In particular, it is shown how, in case of nonsmooth impacts, the inequal- 
ity constraints fi{q{t)) < 0, i = 1, 2, . . . , m, defining the admissible region of 
q{t) can be transformed into equality constraints of the form: 

/i( 9 ( 0 ) +7f(f) = 0 i = l,2,...,m (2.11) 

where 7 i(t) = 0, i = 1, 2, . . . , m are some auxiliary nonnegative real- valued 
variables (the so-called Valentine variables). The derivation with respect to 
time of both sides of (2.11) leads to a set of completely equivalent differential 
constraints, which can be included in the Lagrangian function of the robot (or 
of the mechanical system in general) by means of m Lagrange multipliers. By 
looking for the stationary value of the action integral, according to the Hamil- 
ton principle, the n -|- 2m Euler-Lagrange equations of the system are then 
obtained, and the impact times are determined by the Erdmann- Weierstrass 
corner conditions [25]. 
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The penalty functions method, introduced in the case of smooth impacts, 
is based on the addition of such functions (one per each constraint) to the 
expression of the total potential energy of the system (and then to the La- 
grangian function that appears in the action integral). Each penalty function 
takes the zero value when the corresponding constraint is satished and a 
high positive value when the constraint is violated. In this way, the action 
functional is strongly penalised in correspondence to a violation of the con- 
straints, so that the solution of the modified problem is forced to tend to the 
actual path of motion of the mechanical system, as the penalisation tends to 
infinity. Different choices are possible for the penalty functions; the proposed 
one in [25] is constituted by a sort of fictitious elastic energy, determined by 
a virtual linear elastic spring at the contact point corresponding to the ith 
constraint: 



UfM 



0, if fi{q) < 0 

5^i/?(9), if fi{q) > 0 



(2.12) 



where ki is a sufficiently high positive number, such that Uf^ (q) has the di- 
mension of an energy, when different from zero. An approximation of the 
actual path of motion of the constrained system is then computed by look- 
ing for the unconstrained stationary value of the modified action integral. 
Some points must be highlighted: (i) the condition number of the problem 
of computing the stationary value of the action integral is proportional to 
coefficients ki, i = 1,2, ... ,m, so that the rate of convergence of any nu- 
meric algorithm used to solve the determined equations of motion becomes 
very slow, as the coefficients ki are increased in order to obtain more accu- 
rate solutions; (ii) only .smooth impacts can be approximated by the penalty 
functions method, as the Erdmann- Weierstrass corner conditions obtained in 
this case imply that the generalised velocities cannot have any jump. More 
details about these matters can be found directly in [25] , where some control 
laws are also proposed on the basis of the developed impact models. 



3 Impact Control in Robotics 

Several strategies have been proposed in the most recent years for impact 
control in robotics applications, but no general theory is available, as the 
robot and the environment geometries, together with the type of impact, 
deeply influence not only the form of the equations describing the impacting 
systems, but also the choice of the most suitable control strategy and its 
performances. 

Some fundamental matters must be taken into account in the design of 
an impact control strategy, as: 

— the available knowledge of kinematics and dynamics of the robotic link and 
of the external environment with which it interacts (and in particular the 
knowledge of their stiffness properties); 
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— the need of detecting the impact time, if the adopted control strategy 
is constituted by distinct algorithms in the different phases (free-motion, 
transition, contact); 

— the consequent need of measuring the contact forces by means of suitable 
force or deformation sensors, to detect the impact time and/or to imple- 
ment a force feedback law; 

— the quality of the employed force sensors, and in particular their resolution 
and their dynamic behaviour, to avoid delay in the feedback loop that can 
cause instability phenomena; 

— the possibility of applying alternative control strategies, which do not re- 
quire the impact detection and/or the insertion of force sensors, when the 
above problems limit the achievable performances in unacceptable way. 

A first, general classification of the several impact control schemes that 
have been developed can then be made with reference to the existence or not 
of a switching condition, i.e. with reference to the employment of different 
controllers in the non-contact and contact phases, or of a unique control 
law. In the remainder of the section, different impact control schemes are 
analysed, starting in Section 3.1 from some “old” switching solutions, which 
were developed about a decade ago, mainly on the basis of experimental 
tests. The bouncing problems that may occur, especially when integral control 
laws are employed, are addressed in Section 3.2, where some recent results 
are reported and discussed. The possibility of using a unique control law 
for all the phases (free motion, transition, contact), is briefly discussed in 
Section 3.3, with reference to a scheme that extends the classical impedance 
control solution, and in Section 4, which is devoted to some non-switching 
control solutions, developed by the author and Tornambe [12]-[16]. 

3.1 Some “Old” Experimental Switching Schemes 

In the first 90’s, some interesting works appeared about impact control in 
robotics, presenting some practical and alternative solutions to the classical 
impedance control schemes that were developed e.g. in [8], [18]. In such pa- 
pers, the problem was mainly treated from the experimental point of view, by 
defining switching control strategies that employed different controllers for 
each phase (free motion, transition, contact). The controllers were generally 
optimised in some way for the particular phase, in which each of them was 
applied. 

The main contributions provided by such works are constituted by the 
experimental application of the proposed solutions, and by the carried out 
analysis of the practical properties and problems of the different controllers. 

Among the solutions that were proposed in that period, the control scheme 
developed by Mills and Lokhorst in [21] should be recalled. Such a scheme 
is applicable to n-DOF rigid-link robotic manipulators, for the execution 
of tasks that require transitions from non-contact motion to contact motion 




Impact Modelling and Control of Robotic Links 163 



(and vice versa) with a compliant environment, described by a linear lumped- 
parameter mechanical impedance model. The proposed control strategy is 
constituted by: 

— a PD control law plus a robot dynamics compensation term, during the 
free motion phase; 

— a force/position law, constituted by a PD control law plus a proportional 
force error term plus a robot dynamics compensation term, during the 
contact phase; 

— an indeterminate control input, which is somewhere between the switching 
limits, during the transition phase (in which contact is established with a 
zero contact force). 

The environment compliance is fundamental for the performances of such 
a scheme, which requires the knowledge of the dynamic characteristics of 
both the robot and the environment, under the assumption that no friction 
is present at the contact point. 

The same three different stages of the contact task (i.e. free motion, im- 
pact, and contact after impact) are distinguished in the analysis developed in 
[20]. Here, an experimental comparison of the different control schemes that 
can be used is made for the three stages. The best strategies are combined into 
a single unified controller, basically constituted by a PID controller (based on 
the position error during the free motion phase, and on the force error during 
the contact phase), whose gains are tuned by a knowledge-based tuner that 
makes use of the reference input and the actual position/force information. 
Good experimental results are presented, but no general discussion of the 
stability properties of such a scheme is provided: the main difference with 
respect to the scheme developed in [21] is represented by the introduction 
of an integral term, which is crucial once contact is established in a stable 
way, but which can determine rebounds during the impact phase, as it will 
be discussed with some details in the next subsection. 

3.2 Integral Control Laws and Bouncing Problems 

It is well-known that integral control, which is the best choice for force control 
in an established contact situation (see e.g. [28] and [29]), can determine 
bouncing and instability during the transition phase, because of the integrator 
wind-up, as it has been discussed in [27], where a switching impact control 
strategy, constituted by the following three types of control laws, has been 
proposed: 

— Position control is used for free space motion. 

— Proportional gain explicit force control with negative gain (indicated as 
impact control) is used to suppress oscillations and bouncing, during the 
transient phase of the robot impact with the environment. 
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— Integral gain explicit force control is used to track forces, once stable con- 
tact with the environment has been established. 

Rebounds are avoided by using different solutions to handle the transitions 
between the three controllers: the transition from position control to impact 
control is abrupt and triggered by the impact force spike, whereas the transi- 
tion from impact control to integral force control is done by a linear switch of 
all the gain values, after impact force pulse diminishes (the transition times 
are to be determined empirically). 

More recently, the bouncing problem due to the integrator wind-up has 
been addressed in [6] ; this work shows how integral control can be effectively 
employed for impact control, if some expedients are used. The proposed con- 
trol law is of the following type: 

r = Kf I if- J)dt + Kfxo - K^q, (3.1) 

where r is the motor torque, / is the measured force, / is the force set-point, 
xo is the state of the integrator at the detection of the impact, q is the motor 
velocity, Kf and Ky are the integral and velocity feedback gains. Bouncing 
can be avoided :(i) by choosing a high value for the force set-point (at least 
during the force spike due to impact), as in this way there is no growth of 
the wind-up charge; (ii) by considering an appropriate value for the initial 
integrator state Xq (an empirical expression of xq is given in the paper); (iii) 
by reducing the integral gain when the contact force vanishes, to avoid other 
rebounds after a first possible loss of contact. 

The negative effects that the time delays, induced in the feedback con- 
trol loop, can determine when impact is concerned, have been recently stud- 
ied from the theoretical point of view in [22], in the cases of proportional 
and proportional-integral force feedback. The results of this analysis (and in 
particular the contact instability phenomena) have been experimentally vali- 
dated in [26] , by using a simple test-bed, reproducing the case of a rigid link 
striking a rigid environment. 

An alternative solution to possible bouncing and instability in impact 
control has been proposed in [23] : a positive acceleration feedback is inserted 
to control transient force response, to reduce the peak impulsive force and 
bouncing caused directly by overshooting and oscillation of transient force 
response. Considering the detection of the impact as an event, an event- 
driven switching control strategy is used to deal with the inadvertent loss of 
contact of the robot. The complete control scheme is then constituted by: 

— a nonlinear feedback law, which linearises and decouples the robot dynam- 
ics; 

— a switching controller, which governs the application of the proper control 
law (i.e. the position one or the force one); 
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— a PD position control law, which is applied in the free motion mode, during 
the transition phase, and in the constrained motion mode for the uncon- 
strained directions only; 

— a force control law, which is applied in the constrained directions during the 
contact phase, and constituted by a PI force law plus a weighted positive 
acceleration feedback term. 

Such a control scheme, which has been validated both theoretically and ex- 
perimentally on a 6-DOF PUMA 560 robot arm, has robustness properties 
with respect to different environments, but it requires the complete knowl- 
edge of the robot kinematics and dynamics, and the determination of the 
unconstrained and constrained directions during contact, so that its practi- 
cal implementation may suffer for the same problems, which characterise the 
classical hybrid force/position control schemes. 

3.3 A Unique Control Law for Free Motion and Contact 

The main common, crucial characteristic of all the switching impact control 
schemes, which have been just illustrated, is given by the necessity to cor- 
rectly detect impact (to switch to the proper controller), and then to handle 
the transition phase so to avoid rebounds, also in presence of possible de- 
lays in the control loop. The possibility of applying a unique control law for 
the free-motion phase and the contact one can then be quite appealing to 
avoid this kind of problems. In [19], such a solution has been discussed in 
the case of a redundant manipulator. Here, impact control is performed by 
reducing the impulsive forces at the end effector, both by means of a proper 
solution of robot redundancy and by the application of an impedance con- 
trol scheme. In particular, redundancy is solved by following the augmented 
kinematics approach^ by augmenting the Jacobian matrix on the basis of an 
impact model, which has been derived using the Cartesian space dynamic 
model of the manipulator. The resulting configuration of the redundant ma- 
nipulator gives the smallest amount of impulsive force at the end effector, 
when it follows a prespecified Cartesian trajectory. The impact controller is 
based on a simplified impedance control scheme, in which the inverse of the 
desired inertia matrix is chosen to be identical to the mobility tensor of the 
manipulator in the Cartesian space, in order to reduce the impulsive forces, as 
well as the rebound effects. Only simulation results (no experimental ones) 
are provided for the validation of such a control scheme, so that it is not 
possible to evaluate its actual effectiveness in practice. 



4 Some Proposed Impact Control Schemes 

In this section, two different impact control strategies, which have been devel- 
oped by the author and Tornambe in [12]-[16], are illustrated. Both solutions 
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utilise a unique controller both for the free motion phase and for the contact 
one, so that impact detection is not necessary: no force sensor is then intro- 
duced. It must be immediately stressed that the proposed control schemes 
do not correspond to “true” force controllers: such schemes simply provide 
(at least theoretically, and in the experimental tests that have been carried 
out) a good transition from the non-contact phase to the contact one, for 
all the robotic tasks in which the exact regulation of the contact force to a 
desired value is not required, while contact with unknown environments must 
be guaranteed, with the regulation of the robot to an assigned configuration. 

The first control scheme, illustrated in Section 4.1, allows a stable contact 
of the robot with different environments (i.e. having different elastic/plastic 
characteristics) on the basis of joint positions and velocities measurements, 
without requiring the complete knowledge of the robot inertial parameters 
and of the environment stiffness. 

The second control solution, illustrated in Section 4.2, is constituted by a 
position feedback control law, which has been developed under the assump- 
tion that velocity measures are not available, or that, if available, they may 
be incorrect at the first time instants after the impact, so that they cannot be 
used for control in the crucial transition phase from free motion to contact. 

Both control schemes have been applied to a simple experimental test-bed, 
reproducing the case of a single-link robot striking rigid or elastic environ- 
ments. The set-up, which is schematically reproduced in Figure 4.1, is briefly 
described hereafter. 

The first steel bar, which is actuated by a DC motor, represents a single- 
link robot, while the environment that the link can strike is obtained by 
means of the second steel bar (not coaxial to the first one only for practical 
reasons), whose extremity, involved in the impact, can be: simply rested to 
the steel stroke end to reproduce the case of rigid impacts, covered with 
different materials to obtain quasi-rigid impacts, or connected to the stroke 
end by means of a spring to reproduce the conditions of a pure elastic impact. 

Both bars have the same length, I = 0.16 m; the distance ro between 
their rotational axis (due to the physical dimensions of the motor, which 
could obstruct the positioning of the second bar) is of 0.052 m. The first 
bar (“the link”) is actuated by a brush type DC servo motor (Pittman, # 
GM9413D627), with voltage range of 12 V. Since the motor is a high-speed 
relatively low-torque actuator, it is geared down by a speed reducer, connect- 
ing the motor shaft to the link. The employed reducer, for which negligible 
backlash is ensured, has gear ratio of 127.7:1. 

The output of the system is given by the link position, which is provided 
by an optical incremental encoder, mounted on the motor shaft and having 
a resolution of 400 cycles per revolution of the shaft. The position of the 
actuated link is then provided with a resolution of 400 • 127.7 cycles per 
revolution, corresponding to 7 • 10“^ degrees (i.e. 0.123 mrad). As the encoder 
is of incremental type, a homing procedure is performed at the beginning of 
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each test to fix its zero position, with respect to which the link position will be 
measured during the test. If requested, the link angular velocity is computed 
on line via software from the measured position by a high-gain observer. 



Steel, not actuated, 
environment bar 




covered by cardboard or rubber 



Figure 4.1. Sketch of the experimental test-bed 



A second encoder is mounted on the unactuated bar (“the environment 
bar”) to detect the impact time, and to verify that contact between the link 
and the environment is never lost after the impact, only to evaluate the effec- 
tiveness of the proposed control schemes, but not for their implementation. 

A unique power unit is used for the entire system, containing in particular: 
the servo amplifier to drive the DC motor and the encoders; the digital to 
analog converter for the determination of the command input signal to the 
motor; the counter for the software decoding of the encoders output. The 
system is interfaced for the control with a Pentium MMX 200 MHz Personal 
Computer by a Keithly digital to digital input/output board. The control 
and DSP software are both written in C+-f . 

4.1 A Control Scheme Based on Impact Effects Estimation 

A common approach, which utilises a unique control law for the two phases, 
is the basis of the impact control schemes developed in [12], [13], [15], and 
[24]. This kind of approach is based on the introduction of a reduced-order 
observer for the estimation of the impact induced forces, to be compensated 
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by the feedback control law. An interesting feature of this solution, which 
is illustrated hereafter directly with reference to the case reproduced by the 
experimental set-up, is given by its robustness properties. In fact, only “fea- 
sible” estimates of the robot dynamic parameters are sufficient for its im- 
plementation, whereas the knowledge of the environment stiffness properties 
is necessary only if a precise control force value must be imposed in the 
steady-state, but not to assure a stable contact. 

Let h{q) be the distance between the free extremity of the link and the 
contact surface of the environment bar, given by: 



h{q{t)) = I cos q{t) -I- Tq, 



(4.1) 



where q{t) is the link angular position, Tq is the distance between the ro- 
tational axis of the bars, having length 1. The link is in contact with the 
environment bar when h{q{t)) < 0. The motion equations of the link can 
then be expressed as follows: 

q(t) = j(u{t) - Pq{t)) + dc{t), (4.2) 

where / > 0 is the link inertia, /3 > 0 is the viscous friction coefficient, u{t) 
is the command input applied by the motor, and dc{t) is the impact-induced 
term, given by: 



dc{t) := 



0, if h{q{t)) > 0 

-^Jh{q{t))h{q{t)), if h{q{t)) < 0, 



(4.3) 



where K is the nnknown stiffness coefficient of the environment, and Jh{q) '■= 

dh{q) . 

;r — = —I sinq. 

oq 

The control goal is bringing the link in a stable contact with the environ- 
ment bar, while guaranteeing a proper desired value qr for the steady-state 
position of the link, without using force measurements and/or detecting the 
impact time. 

The proposed control scheme is constituted by a preliminary feedback 
control law: 

u{t) = iu'{t) + $q{t), (4.4) 



where / > 0 and /3 > 0 are some estimates of I and fd, respectively, and by a 
second feedback law, given by: 



u\t) = -ho{q(t) - qr) - hiq{t) - d{t) (4.5) 

d{t) = C(I) + W(I) (4-6) 

i{i) = -M?(I) -Mm'(I), (4-7) 



where ho, hi are positive constants such that the roots of p(A) = X^ + hiX + ho 
are in the open left-half plane, and /r is a positive constant, which can be 




Impact Modelling and Control of Robotic Links 169 

chosen to obtain the global asymptotic stability of the closed-loop system 
(i.e. both in contact and non-contact conditions), as proven in [13]. 

The reduced-order observer, given by (4.6), (4.7), is used to evaluate both 
the impact-induced term dc{t) and the effect of the inexact compensation of 
the inertial and friction terms, performed by the preliminary feedback (4.4), 
by estimating the d{t) term, given by: 

d{t) := {ir - l)u'(f) + y + dc(t), (4.8) 

where Ir := j and Afi := /3 — /?. 

In order to guarantee the asymptotic stability of the closed-loop system, 
the observer gain /r must be greater than a threshold value, which depends 
on K, Ir and Z\/3, and then on I and /3 (see [13] for details). In practice, it 
is not necessary to know them to implement the control scheme, as stability 
is achieved simply if /r is chosen sufficiently high. The weak point of the pro- 
posed control scheme is given by the fact that such a lower threshold value 
of /X increases as K does, so that more rigid contacts should require higher 
gain values, i.e. the controller could be inadequate when the environment is 
in practice highly rigid, and the control law is implemented on a computer 
with a small but not infinitesimal sampling time. Despite of this theoretical 
limitation, the experimental results that are illustrated hereafter (the inter- 
ested reader can find more results in [15]) show that the proposed control law 
can be adopted also when the impacting parts are very rigid (both made of 
steel) . 

As no particular restriction has been made on the estimates I and 13, the 
experimental tests have been carried out by considering 7 = 1 and /3 = 0, so 
that preliminary feedback (4.4) has not been actually performed, as it results 
in this case u{t) = u'{t). The chosen values for the controller and observer 
gains are: ho = 100, h\ =20, = 0.1; the sampling time is 0.005 s. 

In the first carried out test, the environment bar is connected to the steel 
stroke end by means of a spring of unknown stiffness coefficient; the assigned 
reference link position, which is equal to 0.0942 rad, corresponds to a link 
rotation of 40 encoder impulses with respect to the zero position fixed by the 
initial homing procedure. The results reported in Figures 4.2 and 4.3 show 
that after the link impact against the environment bar, occurring after about 
0.03 s (see Figure 4.3 (a)), the contact between the bars is never lost (see 
Figures 4.2 (a), 4.3 (a)), and the desired final position is actually reached by 
the link (see Figure 4.3 (b)). 

No force measurement is available, but the d{t) term, evaluated by the 
observer (4.6), (4.7) and reported in Figure 4.2 (d), can be considered as an 
estimation of the effect of the impact induced forces on the command voltage 
during the contact phase. Only by the knowledge of the stiffness coefficient 
of the spring (not available in our case), the actual contact force value could 
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be computed from d{t), under the assumption that other disturbances (e.g. 
friction) are negligible during the contact phase. 



(a) Link displacement 





time (s) 



(b) Link velocity 




(d) Estimated impact effect 




Figure 4.2. Experimental results: elastic impact, with the controller + observer 
scheme 



From the theoretical point of view, the most critical situation for the 
proposed control scheme is represented by the case of a rigid impact. The 
results reported in Figures 4.4 and 4.5 are relative to the second carried ont 
test, in which the environment bar is simply rested to the stroke end (without 
the insertion of any elastic element), and no material covers the steel bars: 
the impact can then be considered as rigid. The imposed reference value for 
the link position is now given by 0.1885 rad, defined with respect to the zero 
position fixed by the new initial homing procednre (note that the absence 
or presence of the spring changes the position of the environment bar); the 
controller and observer gains are kept unchanged. 

As the results reported in Figures 4.4 and 4.5 show, also in this test the 
contact between the link and the environment bar, which is established after 
about 0.05 s, is indefinitely maintained (see in particular Figures 4.4 (a) 
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Figure 4.3. Experimental results: elastic impact, with the controller + observer 
scheme 



and 4.5 (a)), and the desired link position is finally achieved, as shown by 
Figure 4.5 (b). As it was reasonable to expect, the effect of the impact induced 
force, given by the estimated term d{t), is larger than in the elastic case, 
especially in the first time instants after the impact (compare Figures 4.2 (d) 
and 4.4 (d)), but the corresponding control voltage is anyway largely tolerable 
by the system (see Figures 4.2 (c) and 4.4 (c)). 

4.2 A Position Feedback Control Law 

In papers [14], [16], a unique feedback control law has been proposed for 
simple mechanical systems subject to impacts (like the one represented by our 
experimental test-bed), by using only the output, given by the position of the 
actuated impacting element (the link in our case) . Such a choice is motivated 
by the fact that the velocity measures, which are often available for the 
mechanical systems, can be incorrect at the first time instants, immediately 
after the impact. 

For the sake of simplicity, the proposed control strategy is derived by 
considering a simplified representation of a mechanical system subject to 
impacts, and then directly extended to the single-link case of our experimental 
test-bed. 

Consider a mass m, moving along a straight line, due to the action of a 
command force fc{t). The position of the mass is represented by q{t) and 
measured with respect to a planar surface (located in g = 0), orthogonal to 
the motion line of the mass. The contact between the mass and the surface 
is established for all t such that q{t) < 0. The impact between the mass and 
the surface (described by a high stiffness coefficient K) is then defined as the 
transition from the mass free motion condition to the constrained motion one. 
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(a) Link displacement 





(b) Link velocity 




(d) Estimated impact effect 




Figure 4.4. Experimental results: rigid impact, with the controller + observer 
scheme 



(a) Impact time (dashed) (b) Displacement error 





Figure 4.5. Experimental results: rigid impact, with the controller + observer 
scheme 
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at a nonzero negative velocity, i.e. impact occurs at a certain time instant U, 
if q{ti) = 0 and q{ti) < 0. 

By considering y[t) = q{t) as output, and u{t) = ;^/c(t) as input, the 
following transfer functions Pi{s) Y{s)/U{s), i = 1,2, can be defined in 
the two non-contact/contact situations: 

^ (non-contact) P 2 {s) = „ ^ , (contact), (4.9) 

+ k 

where k := Kim. 

The control goal, to be performed by developing a unique position feed- 
back law, whose structure is independent of the motion phase (contact or 
non-contact), is to bring the mass in a stable contact with the surface, while 
achieving the practical regulation of the mass position to an assigned ref- 
erence value (corresponding to a desired contact force, if K is known), and 
guaranteeing a satisfactory insensitivity of the system behaviour to the im- 
pact effects, i.e. with a small force overshoot due to the impacts. 

As for the control scheme illustrated in the previous subsection, the con- 
trol goal is expressed with reference to a desired “contact” position, since no 
force sensor is assumed to be used; besides, in this case, as no estimation 
of the impact induced forces or effects is performed, the goal is intended to 
be completely accomplished if the controlled system automatically reduces 
the impact effects in a satisfying way for the requested task, and for the 
preservation of the mechanical system itself. 

The first step to be performed for the design of the control feedback law 
is checking if systems Pi{s) and P 2 {s) are simultaneously stabilisable. By 
introducing coprime rational factorisations of Pi{s) and P 2 {s) of the form 
Pi{s) = N Pi{s) / D Pi{s) , i = 1,2, where Npi{s), Dpi{s) G TZ (with TZ being 
the set of the proper rational functions, having all the poles in the open left- 
half plane), it is possible to prove [4], [5] that there exists a one-parameter 
family of compensators G(s), such that the closed- loop system correspond- 
ing to the open-loop transfer function G{s)Pi{s) in the non-contact case, 
and to G{s)P 2 {s) in the contact one, is internally asymptotically stable in 
both phases, i.e. systems Pi{s) and P 2 {s) can be simultaneously stabilised 
(see details in [16]). It follows then that the global closed- loop system thus 
obtained is internally asymptotically stable if a finite number of impacts oc- 
curs, i.e. if a finite number of switches between the transfer functions in (4.9) 
occurs. If it is not possible to prove that the number of impacts is finite, the 
asymptotic stability of the mechanical system in the two separate phases (the 
non-contact and the contact one) does not imply the asymptotic stability of 
the global system, as it is strongly dependent on how the system commutes 
between the two different phases, and a deeper investigation is then needed. 

For the subsequent developments, it is useful to define the term: 



Jo if q{t) > 0 (non-contact) 
J kq{t) if q{t) < 0 (contact), 



(4.10) 
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which is proportional to the impact induced force. 
By inserting the control law: 



u{s)=G{s){v{s)-y{s)), (4.11) 



where v{s) is the Laplace transform of the reference signal v{t), and G(s) 
is the transfer function of the compensator to be determined, the following 
closed-loop system is obtained: 



e(s) = 



l + Pi(s)G(s 



-v{s) 



Pijs) 

l + Pi{s)G{s 



■6{s), 



(4.12) 



where e(s) is the Laplace transform of the tracking error e{t) := v{t) — y{t). 

To accomplish the previously described control goal, G(s) must be deter- 
mined so that: 



a) the closed-loop system is asymptotically stable (independently of the num- 
ber of impacts); 

b) lTe(s) := has at least a zero in s = 0, to guarantee that the 

1 -f Pi(s)G(s) 

tracking error is null at the steady-state, for any constant reference input 
v{t) = V] 

c) Wd(s) := , has at least a zero in s = 0 too, for the tracking 

1 + Pi(s)G(s) 

error to be equal to zero at the steady-state, also in presence of nonzero 
steady-state values of the contact force, and it is such that the impact 
effects on the measured output are reduced as much as possible during the 
transient time (in the ideal case it would be desirable to have no effect at 
all). 

It can be proven [5] that the set of all the compensators G(s) that stabilises 
Pi(s) can be parameterised as follows: 



G(s) = 



4li(s) + Q{s)Ci{s) 
Pi(s) + Q(s)Pi(s)’ 



(4.13) 



where Ai(s), Pi(s), Gi(s), and Di{s) are defined by the previously intro- 
duced coprime rational factorisation of Pi(s), according to the Bezout iden- 
tity [5], and Q{s) G TZ is such that Li(s) = Pi(s) -I- Q{s)Di{s) is biproper 
(as Pi(s) is strictly proper, Pi(s) is surely biproper for all Q{s) G TZ). 

The compensator G(s) given in (4.13) certainly stabilises Pi(s) and (it can 
be easily verified) satisfies property b), whereas the ideal choice for (5(s), to 
fully satisfy the properties requested by c), would be given by a not proper, i.e. 
not implementable, function Qid{s) of the form: Qid(s) = —{kos'^ + kiS + k 2 ), 
where ki, z = 0, 1, 2 are determined by the used factorisation of Pi(s). Q{s) 
can then be defined so as to approximate the ideal function Qid{s) at least 
in the operating frequency bandwidth [0,1?], where 1? is a sufficiently high 
frequency; a possible choice for Q{s) is given by: 
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Q{s) 



(fcos^ + kis + ^2) 
(1 + TiS)(1 + T2S)’ 



(4.14) 



where the time constants n, T 2 of the poles must be such that — , — ^ f2. 

Ti T2 ^ 

As it is not possible to know if a finite number of impacts occurs (i.e. 
a finite number of switches between Pi(s) and P 2 {s)), a sufficient condition 
for the asymptotic stability of the entire closed-loop system can be found by 
determining a unique Lyapunov function for the closed-loop system in the 
two phases, once a state-space realisation has been introduced for G{s) (see 
details and simulation results in [16]). 

Th effectiveness of such a control solution has been validated by using our 
experimental test-bed in the case of rigid impacts (i.e. with the environment 
bar simply rested to the stroke end). The command input has been defined 
from (4.13) in the following form: 



u{s) = Kp{v - y{s)) - sKoyis) -b aygis), (4.15) 

in which yqis) is the output of the system that realises the transfer function 
Q{s) given in (4.14), whereas the value of the coefficient a depends on the 
link inertia. 



Link position [rad] Link veiooity [rad/s] 





Figure 4.6. Experimental results: rigid impact, with the position feedback control 
scheme (with Kp = 100, Kd = 20, n = t 2 = 0.5 s) 



The sampling time used for the discrete-time implementation of the con- 
troller is 0.005 s, as in the previous tests, whereas the discretisation has been 
performed by using the first order Euler approximation rule. 

A link rotation of 190 encoder impulses, corresponding to a reference 
position value (compatible with the contact of the link with the environment 
bar) of 0.5969 rad, is imposed, by considering Kp = 100, Kp = 20 in (4.15). 
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Figure 4.6 shows the obtained time-history of the link position and velocity 
without the insertion of the r/g(s) term in (4.15) (reported by a thin line), 
and with the insertion of yg(s) (reported by a bold line), corresponding to 
the output of 



Qis) 



4s 2 -h 86s -k 523 
s^ -I- 4s -I- 4 



i.e. with Ti = T 2 = 0.5 s. 

As expected by the previously developed analysis, the link is led towards 
the environment bar, and a stable contact is established between them. The 
insertion of the yg term allows a significant reduction of the impact effects, 
corresponding to a reduction of the link maximum velocity of about 23%, 
and also a reduction of the steady-state position tracking error (the desired 
position value is reported in the hgure by a dashed line). No force time-history 
can be reported, as no force sensor has been employed, and the stiffness 
properties of the environment bar are not exactly known. 

To evaluate the influence of the yq term on the reduction of the link 
maximum velocity, the same reference position has been considered in a new 
test, in which the choices Kp = 300 and Kp> = 5 have been made in (4.15) 
(i.e. a very small damping action has been introduced in the PD part of the 
command law). The choice ti = T 2 = 0.5 s has been maintained for Q{s). 
Figure 4.7 shows the obtained time-history of the link position and velocity 
without the insertion of the j/g(s) term in (4.15) (reported by a thin line), 
and with the insertion of yg(s) (reported by a bold line). 



Link position [rad] Link velocity [rad/s] 





Figure 4.7. Experimental results: rigid impact, with the position feedback control 
scheme (with Kp = 300, Kp = 5, n = t 2 = 0.5 s) 



As the hgure shows, also in this case a stable contact between the bars is 
achieved, together with a signihcant reduction of the impact effects when the 
yq term is inserted. In particular, a reduction of the link maximum velocity 
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of about 25% is obtained, and a reduced tracking error is achieved, with only 
a negligible position overshoot with respect to the assigned reference value 
(reported in the figure by a dashed line). 

The obtained results have shown then the effectiveness of the proposed 
approach, despite of all the factors that have been neglected in the developed 
analysis (such as any form of friction and stiction, the coefficient of restitu- 
tion, etc.). In particular, they have shown that, even if the exact achievement 
of the desired link position can be prevented in practice, mainly due to fric- 
tion, maintenance of a stable contact is assured with a satisfying reduction 
of the impact effects. 



5 Conclusions 

Interesting and quite good results have been obtained during the last years in 
the impact control field, but the problem is still quite open. Performance of 
the presented schemes can depend on various elements, such as the quality of 
the used sensors and actuators, the actual knowledge of the systems involved 
in the impact, and the impact type itself, so that the choice of the best 
strategy for each application is often a delicate task, that the designer must 
afford taking into account all the technological characteristics and limitations 
of the considered plant. 

Finally, a further deeper analysis should be made for the micro-robotics 
applications and for the robots having flexible links, since the reduced di- 
mensions and the distributed elasticity properties of the involved structures 
should probably imply a more detailed description of the impact phenomena, 
for the subsequent design of suitable control strategies. 
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The subject of this chapter is the motion control problem of wheeled mobile 
robots (WMRs). With reference to the unicycle kinematics, we review and 
compare several control strategies for trajectory tracking and posture sta- 
bilization in an environment free of obstacles. Experiments are reported for 
SuperMARIO, a two- wheel differentially-driven mobile robot. From the com- 
parison of the obtained results, guidelines are provided for WMR end-users. 



1 Introduction 

Wheeled mobile robots (WMRs) are increasingly present in industrial and 
service robotics, particularly when flexible motion capabilities are required 
on reasonably smooth grounds and surfaces [29]. Several mobility config- 
urations (wheel number and type, their location and actuation, single- or 
multi-body vehicle structure) can be found in the applications, e.g, see [18]. 
The most common for single-body robots are differential drive and synchro 
drive (both kinematically equivalent to a unicycle), tricycle or car- like drive, 
and omnidirectional steering. A detailed reference on the analytical study of 
the kinematics of WMRs is [2] . 

Beyond the relevance in applications, the problem of autonomous motion 
planning and control of WMRs has attracted the interest of researchers in 
view of its theoretical challenges. In particular, these systems are a typical 
example of nonholonomic mechanisms due to the perfect rolling constraints 
on the wheel motion (no longitudinal or lateral slipping) [24]. 

In the absence of workspace obstacles, the basic motion tasks assigned 
to a WMR may be reduced to moving between two robot postures and fol- 
lowing a given trajectory. From a control viewpoint, the peculiar nature of 
nonholonomic kinematics makes the second problem easier than the first; in 
fact, it is known [8] that feedback stabilization at a given posture cannot be 
achieved via smooth time-invariant control. This indicates that the problem 
is truly nonlinear; linear control is ineffective, even locally, and innovative 
design techniques are needed. 

After a preliminary attempt at designing local controllers, the trajectory 
tracking problem was globally solved in [28] by using a nonlinear feedback 
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action, and independently in [12] and [11] through the use of dynamic feed- 
back linearisation. A recursive technique for trajectory tracking of nonholo- 
nomic systems in chained form can also be derived from the backstepping 
paradigm [17]. As for posture stabilization, both discontinuous and/or time- 
varying feedback controllers have been proposed. Smooth time- varying stabi- 
lization was pioneered by Samson [26], [27], while discontinuous (often, time- 
varying) control was used in various forms, e.g. see [1], [10], [21], [22], [32]. A 
recent addition to this class was presented in [14], where dynamic feedback 
linearisation has been extended to the posture stabilization problem. 

While comparative simulations of several of the above methods are given 
in [9] for a unicycle and in [13] for a car-like vehicle, there is no extensive 
experimental testing on a single benchmark vehicle. The objective of this 
chapter is therefore to evaluate and compare the practical design and perfor- 
mance of control methods for trajectory tracking and posture stabilization, 
highlighting potential implementation problems related to kinematic or dy- 
namic nonidealities, e.g. wheel slippage, discretisation and quantization of 
signals, friction and backlash, actuator saturation and dynamics. 

All control designs are directly presented for the case of unicycle kinemat- 
ics, the most common among WMRs, and experimentally tested on the lab- 
oratory prototype SuperMARIO. Nonetheless, most of the methods selected 
for comparison can be generalised to vehicles with more complex kinematics. 

1.1 Organization of Contents 

In Section 2 we classify the basic motion control tasks for WMRs. The mod- 
eling and main control properties are summarised in Section 3. In Section 4, 
the experimental setup used in our tests is described in detail. 

Trajectory tracking controllers are presented in Section 5. After discussing 
the role of nominal feedforward commands (Section 5.1), three feedback laws 
are illustrated. They are based respectively on tangent linearisation along the 
reference trajectory and linear control design (Section 5.2), on a nonlinear 
Lyapunov-based control technique (Section 5.3), and on the use of dynamic 
feedback linearisation (Section 5.4). Comparative experiments on exact and 
asymptotic trajectory tracking are conducted in Section 5.5, using an eight- 
shaped desired trajectory. 

The posture stabilization problem to the origin of the configuration space 
is considered in Section 6. Four conceptually different feedback methods 
are presented, using time-varying smooth (Section 6.1) or nonsmooth (Sec- 
tion 6.2) control laws, a discontinuous controller based on polar coordinates 
transformation (Section 6.3), and a stabilizing law based on dynamic feedback 
linearisation (Section 6.4). Results on forward and parallel parking experi- 
ments are reported. 

Finally, in Section 7 the obtained results are summarised and compared in 
terms of performance, ease of control parameters tuning, sensitivity to non- 
idealities, and generalisability to other WMRs. In this way, some guidelines 
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are proposed to end-users interested in implementing control laws for WRMs. 
Open problems for further research are pointed out. 



2 Basic Motion Tasks 

The basic motion tasks that we consider for a WMR in an obstacle-free 

environment are (see Figure 2.1): 

— Point-to-point motion: The robot must reach a desired goal configuration 
starting from a given initial configuration. 

— Trajectory following: A reference point on the robot must follow a trajec- 
tory in the Cartesian space (i.e. a geometric path with an associated timing 
law) starting from a given initial configuration. 




(a) 




Figure 2.1. Basic motion tasks for a WMR: (a) point-to-point motion; (b) trajec- 
tory following 



Execution of these tasks can be achieved using either feedforward commands, 
or feedback control, or a combination of the two. Indeed, feedback solutions 
exhibit an intrinsic degree of robustness. However, especially in the case of 
point-to-point motion, the design of feedback laws for nonholonomic systems 
has to face a serious structural obstruction, as we will show in Section 3; 
controllers that overcome such difficulty may lead to unsatisfactory transient 
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performance. The design of feedforward commands is instead strictly related 
to trajectory planning, whose solution should take into account the specihc 
nonholonomic nature of the WMR kinematics. 

When using a feedback strategy, the point-to-point motion task leads to a 
state regulation control problem for a point in the robot state space — posture 
stabilization is another frequently used term. Without loss of generality, the 
goal can be taken as the origin of the n-dimensional robot configuration 
space. As for trajectory following, in the presence of an initial error (i.e. an 
off-trajectory start for the vehicle) the asymptotic tracking control problem 
consists in the stabilization to zero of Cp = {ex,€y), the two-dimensional 
Cartesian error with respect to the position of a moving reference robot (see 
Figure 2.1b). 

Contrary to the usual situation, tracking is easier than regulation for a 
nonholonomic WMR. An intuitive explanation of this can be given in terms 
of a comparison between the number of controlled variables (outputs) and 
the number of control inputs. For the unicycle-like vehicle of Section 3, two 
input commands are available while three variables {x, y, and the orientation 
6) are needed to determine its configuration. Thus, regulation of the WMR 
posture to a desired configuration implies zeroing three independent config- 
uration errors. When tracking a trajectory, instead, the output Cp has the 
same dimension as the input and the control problem is square. 



3 Modelling and Control Properties 

Let q G Q he the n-vector of generalised coordinates for a wheeled mobile 
robot. Pfaffian nonholonomic systems are characterised by the presence of 
n — m non-integrable differential constraints on the generalised velocities of 
the form 

A(q)q = 0. (3.1) 

For a WMR, these arise from the rolling without slipping condition for the 
wheels. All feasible instantaneous motions can then be generated as 

q = G{q)w, w G (3.2) 

where the columns gi, i = 1, . . . ,m, oi the n x m matrix G{q) are chosen so 
as to span the null space of matrix A{q). Different choices are possible for 
G, according to the physical interpretation that can be given to the ‘weights’ 
tCi, . . . ,Wm- Equation (3.2), which is called the (first-order) kinematic model 
of the system, represents a driftless nonlinear system. 

The simplest model of a nonholonomic WMR is that of the unicycle, 
which corresponds to a single upright wheel rolling on the plane (top view in 
Figure 3.1). The generalised coordinates are q = {x,y,9) £ Q = x SO^ 
(n = 3). The constraint that the wheel cannot slip in the lateral direction is 
given in the form (3.1) as 
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Figure 3.1. Relevant variables for the unicycle (top view) 



isin0 — ycos9 — 0. 

A kinematic model is thus 

X cos 9 0 

y = 9 i{(l)v + g 2 {q)x> = sin0 v+ 0 w, (3.3) 

9 \ L 0 J [ 1 

where v and w (respectively, the linear velocity of the wheel and its angular 
velocity around the vertical axis) are assumed as available control inputs 
(m = 2). As we will show in Section 4, this model is equivalent to that of 
SuperMARIO. 

System (3.3) displays a number of structural control properties, most of 
which actually hold more in general for (3.2). 

3.1 Controllability at a Point 

The tangent linearisation of (3.3) at any point qe is the linear system 
cos 9g 0 

q = sin 0e V + 0 u> q = q — qe, 

0 J [ 1 

that is clearly not controllable. This implies that a linear controller will never 
achieve posture stabilization, not even in a local sense. In order to study the 
controllability of the unicycle, we need therefore to use tools from nonlinear 
control theory [16]. It is easy to check that the accessibility rank condition is 
satisfied globally (at any qe), since 



rank [51 52 [gi, 92 ]] = 3 = n. 



(3.4) 
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being the Lie bracket [gi , 32 ] of the two input vector fields 
r 1 dg 2 dgi 

Since the system is driftless, condition (3.4) implies its controllability. 

Controllability can also be shown constructively, i.e. by providing an ex- 
plicit sequence of maneuvers bringing the robot from any start configuration 
{Xs,Vs,0s) to any desired goal configuration (xg,yg,9g). Since the unicycle 
can rotate on itself, this task is simply achieved by an initial rotation on 
{Xs,ys) until the unicycle is oriented toward (Xg,yg), followed by a transla- 
tion to the goal position, and by a hnal rotation on {xg,yg) so as to align 9 
with 9g. 

As for the stabilisability of system (3.3) to a point, the failure of the pre- 
vious linear analysis indicates that exponential stability cannot be achieved 
by smooth feedback [31]. Things turn out to be even worse: if smooth (in 
fact, even continuous) time- invariant feedback laws are used, Lyapunov sta- 
bility is out of reach. This negative result is established on the basis of a 
necessary condition due to Brockett [6]: smooth stabilisability of a driftless 
regular system (i.e. such that the input vector fields are well defined and 
linearly independent at Qe) requires a number of inputs equal to the number 
of states. 

The above obstruction has a deep impact on the control design. In fact, 
to obtain a posture stabilizing controller it is either necessary to give up 
the continuity requirement and/or to resort to time-varying control laws. In 
Section 6 we shall pursue both approaches. 



sin 9 
— cos 9 
0 



3.2 Controllability About a Trajectory 



Given a desired Cartesian motion for the unicycle, it may be convenient 
to generate a corresponding state trajectory qd{t) = {xd{t),yd{t),9d{t)). In 
order to be feasible, the latter must satisfy the nonholonomic constraint on 
the vehicle motion or, equivalently, be consistent with (3.3). The generation 
of qd{t) and of the corresponding reference velocity inputs Vd{t) and uJd{t) 
will be addressed in Section 5. 

Defining the state tracking error as q = q — qd and the input variations as 
V = V — Vd and u) = lo — u>d, the tangent linearisation of system (3.3) about 
the reference trajectory is 



0 0 -Vd sin9d 




cos 9d 0 




r ~ 1 


0 0 Vd cos 9d 


q + 


sin 9d 0 




V 


0 0 0 




0 1 




iV 



A{t)q + B{t) 



V 

U) 



(3.5) 
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Since the linearised system is time-varying ^ a necessary and sufhcient con- 
trollability condition is that the controllability Gramian is nonsingular. How- 
ever, a simpler analysis can be conducted by defining the state tracking error 
through a rotation matrix as 






cos 6d sin 9d 0 
— sin 9d cos 9d 0 
0 0 1 



q- 



Using (3.5), we obtain 



r 0 Ud 0 1 




■ 1 o' 




r ~ 1 


o 

1 


qn + 


0 0 




V 


[ 0 0 0 




0 1 




UJ 



(3.6) 



When Vd and cOd are constant, the above linear system becomes time-invariant 
and controllable, since matrix 



C = [B AB A^B] = 



1 0 0 0 -toj Vduid 

0 0 -LOd Vd 0 0 

0 1 0 0 0 0 



has rank 3 provided that either Vd or u>d are nonzero. Therefore, we conclude 
that the kinematic system (3.3) can be locally stabilised by linear feedback 
about trajectories which consist of linear or circular paths, executed with 
constant velocity. 

In Section 5 we shall see that it is possible to use linear design tech- 
niques in order to obtain local stabilization for arbitrary feasible trajectories, 
provided they do not come to a stop. 



3.3 Feedback Linearisability 

Based on the previous discussion, it is easy to see that the driftless nonholo- 
nomic system (3.2) cannot be transformed into a linear controllable one using 
static state feedback. In particular, for the unicycle (3.3) the controllability 
condition (3.4) implies that the distribution generated by vector fields gi 
and (?2 is not involutive, thus violating the necessary condition for full state 
feedback linearisability [16]. 

However, when matrix G{q) in (3.2) has fnll column rank, m equations can 
always be transformed via feedback into simple integrators (input-output lin- 
earisation and decoupling). The choice of the linearising outputs is not unique 
and can be accommodated for special purposes. An interesting example is the 
following. Define the two outputs as 

yi = X + b cos 9 
V 2 = y-\-bsin9, 

with b ^ 0, i.e. the Cartesian coordinates of a point B displaced at a distance 
b along the main axis of the unicycle (see Figure 3.1). 
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Using the globally defined state feedback 



V 




cos 9 sin 9 




Ml 


UJ 




— sind/6 cos d/6 




U2 



the unicycle is equivalent to 

yi = ui 
U2 

U 2 cos 9 — ui sin 9 

b ■ 

As a consequence, a linear feedback controller for u = (ui,U 2 ) will make the 
point B track any reference trajectory, even with discontinuous tangent to 
the path (e.g. a square without stopping at corners). Moreover, it is easy 
to show that the internal state evolution 9{t) is bounded. This approach, 
however, will not be pursued in this chapter because of its limited interest 
for more general kinematics. 

For exact linearisation purposes, one may also resort to the more general 
class of dynamic state feedback. In this case, the conditions for full state 
linearisation are less stringent and turn out to be satisfied for a large class of 
nonholonomic WMRs. However, there is a potential control singularity that 
has to be considered carefully. The use of dynamic feedback linearisation will 
be illustrated later both for asymptotic trajectory tracking (Section 5) and 
for posture stabilization (Section 6). 

3.4 Chained Forms 

The existence of canonical forms for kinematic models of nonholonomic robots 
allows a general and systematic development of both open-loop and closed- 
loop control strategies. The most useful canonical structure is the chained 
form, which in the case of two-input systems is 



Zl 


= Ml 




Z2 


= M2 




h 


= Z2Ui 


(3.7) 




= Zn-lUi. 





It has been shown that a two-input driftless nonholonomic system with up to 
n = 4 generalised coordinates can always be transformed in chained form by 
static feedback transformation [23]. As a matter of fact, most (but not all) 
WMRs can be transformed in chained form. 

For the kinematic model (3.3) of the unicycle, we introduce the following 
globally defined coordinate transformation 
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e 

X cos 9 + ysinO 
xsuyO — y cos 9 

and static state feedback 



^1 = 
^2 = 
= 



obtaining 



V = U2+ Z3U1 

to = Ui, 



(3.8) 



ii = ui 

Z2 = U2 (3.9) 

Z3 = Z2U1. 



Note that ( 22 ,^ 3 ) is the position of the unicycle in a rotating left-handed 
frame having the Z 2 axis aligned with the vehicle orientation (see Figure 3.1). 
Equation (3.9) is another example of static input-output linearisation, with 
Zi and Z 2 as linearising outputs. We note also that the transformation in 
chained form is not unique (see, e.g. [9]). 



4 Target Vehicle: SuperMARIO 

The experimental comparison of the control methods to be reviewed in this 
chapter has been performed on the mobile robot SuperMARIO, built in the 
Robotics Laboratory of our Department (Figure 4.1). 

4.1 Physical Description 

SuperMARIO is a two-wheel differentially-driven vehicle, a mobility config- 
uration found in many wheeled mobile robots. The two wheels have radius 
r = 9.93 cm and are mounted on the same axle of length d = 29 cm. The 
wheel radius includes also the o-ring used to prevent slippage; the rubber 
is stiff enough that point contact with the ground can be assumed. A small 
passive off-centered wheel is used as a caster, mounted in the front of the 
vehicle at a distance of 29 cm from the rear axle. The aluminum chassis of 
the robot measures 46 x 32 x 30.5 cm (1/w/h) and contains two motors, trans- 
mission elements, electronics, and four 12 V batteries. The total weight of the 
robot (including batteries) is about 20 kg and its center of mass is located 
slightly in front of the rear axle. This design choice limits the disturbance on 
robot motion induced by sudden reorientation of the caster. Each wheel is 
independently driven by a DC servomotor (by MCA) supplied at 24 V with a 
peak torque of 0.56 Nm. Each motor is equipped with an incremental encoder 
counting rig = 200 pulses/turn and a gearbox with reduction ratio = 20. 
On-board electronics multiplies by a factor m = 4 the number of pulses/turn 
of the encoders, representing the angular increments with 16 bits. 
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Figure 4.1. The wheeled mobile robot SuperMARIO 



SuperMARIO is a low-cost prototype and presents therefore the typical 
nonidealities of electromechanical systems, namely friction, gear backlash, 
wheel slippage, actuator deadzone and saturation. These limitations clearly 
affect control performance. In addition, all controllers have been designed 
on the basis of a purely kinematic model. However, due to robot and ac- 
tuator dynamics (masses and rotational inertias), velocity commands with 
discontinuous prohle will not be exactly realised by the vehicle. 

4.2 Control System Architecture 

SuperMARIO has a two-level control architecture (see Figure 4.2). High-level 
control algorithms (including reference motion generation) are written in 0“'"“'' 
and run with a sampling time of Ts = 50 ms on a remote server (a 300 MHz 
Pentium II), which also provides a user interface with real-time visualization 
as well as a simulation environment. The PC communicates through a radio 
modem with serial communication boards on the robot. The maximum speed 
of the radio link is 4800 bit/s. Wheel angular velocity commands u>l and ur 
are sent to the robot and encoder measures A<j)L and A(pR are received for 
odometric computations. 

The low-level control layer is in charge of the execution of the high-level 
velocity commands. For each wheel, an 8-bit ST6265 microcontroller imple- 
ments a digital PID with a cycle time of Tc = 5 ms. Two power amplihers 
drive the motors with a 51 KHz PWM voltage. 
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Figure 4.2. Control architecture of SuperMARIO 



Custom interpolation algorithms have been developed on the PC so as 
to reduce the effect of quantization errors and communication delays in the 
reconstruction of the robot posture from the odometric information provided 
by the encoders. For all control schemes, an additional filtering of high-level 
velocity commands is included to account for vehicle and actuator dynamics. 
Simple first-order linear filters smooth possible discontinuities in the velocity 
profiles. 



4.3 Kinematics 



The kinematic model of SuperMARIO is given by (3.3), i.e. is equivalent to 
that of a unicycle. However, the actual commands are the angular velocities 
Ur and wl of the right and left wheel, respectively, rather than the driving 
and steering velocities v and u. There is, however, a one-to-one mapping 
between these velocities: 

” = 2 " = d ■ <“> 

A calibration procedure has also been developed to estimate the actual wheel 
radii and axle length. 

The reconstruction of the current robot configuration is based on in- 
cremental encoder data (odometry). Let A(j)R and AcpR be the angular 
wheel displacements measured during the sampling time Tg by the encoders. 
From (4.1), we obtain the linear and angular displacements of the robot as 

Z\s = ^ {A(j>R + A4 >l) ~ ■ 

The estimate of the posture at time tk = kTg is computed as 
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where^ 

Ok = Ok-i H — —■ 

Robot localization using the above odometric prediction (commonly referred 
to as dead reckoning) is accurate enough in the absence of wheel slippage and 
backlash. These effects are however largely reduced when the velocity is kept 
reasonably small and the number of backup maneuvers is limited. 



4.4 Control Constraints 



Because of the bounded velocity capability of the motors, each wheel can 
achieve a maximum angular velocity f2. Through (4.1), we obtain bounds on 
the driving and steering velocities: 

II II 2J?r 

\v\ < fir, |w| < 

There is, however, a more stringent constraint due to the limited reso- 
lution of the digital low-level control layer. In fact, the linear displacement 
resolution Asmin of the robot can be computed from the previous data as 



As 



min 



2ttt 

m He Ur 



19.86 7T 
4 • 200 • 20 



~ 0.0039 cm. 



This value corresponds to the least significant bit of the encoder, so that 
the average quantization error will be less than 2 hundreds of mm. In view of 
the 8-bit resolution of the on-board velocity microcontroller and of the PWM 
circuit (having /m = l/^c = 200 Hz as minimum pulse frequency), the actual 
linear velocity command has the following threshold and saturation levels 



Vm = fmAsmin ^ 0.78 cm/s Vm =Vm' (2® “ l) = 198.9 cm/s. 



To prevent as much as possible wheel slippage, in our control software 
we have imposed the following even more conservative bounds on high-level 
velocity commands: 



\v\ < Vmax = 0.3 m/s |u;| < LOraax = 0.5 rad/s. 



With these saturations, it is necessary to perform a suitable velocity scaling 
so as to preserve the curvature radius corresponding to the nominal velocities 
V and to. The actual commands Vc and Uc are then computed by defining 



a — max 



u 






,1 



v„ 



^max ^max 

and letting Vc = v and Wc = if cr = 1, while 

if (7 — then Vc = sign(u) Vmax, oJc = w/ct, 

else Vc = v /a, ujc = sign(w) ujmax- 



^ Use of the average value 9k of the robot orientation is equivalent to the numerical 
integration of (3.3) via a 2nd order Runge-Kutta method. 
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5 Trajectory Tracking 

The solution of the asymptotic tracking problem requires the combination of 
a nominal feedforward command with a feedback action on the error. In the 
control schemes to be presented, this error will be defined with respect to ei- 
ther the reference output trajectory (output error) or an associated reference 
state trajectory (state error). 

5.1 Feedforward Command Generation 

Assume that the representative point {x, y) of the unicycle must follow the 
Cartesian trajectory {xd{t),yd{t)), with t G [0,T] (possibly, T oo). From 
the kinematic model (3.3) one has 

6* = ATAN2 (y, i) -b fcTT /c = 0, 1, (5-1) 

where ATAN2 is the four-quadrant inverse tangent function (undefined only 
if both arguments are zero). Therefore, the nominal feedforward commands 
are 



Vd{t) = ±^J xlit) + yj{t) 


(5.2) 


yd{t)xd{t) - Xd{t)yd{t) 


(5.3) 



having differentiated (5.1) w.r.t. time in order to compute ujd- The chosen sign 
for Vd{t) will determine forward or backward motion of the vehicle. We note 
that, in order to be exactly reproducible using Vd{t) and Ud{t), the desired 
Cartesian motion {xd{t),yd{t)) should be twice differentiable in [0,T]. 

A remarkable property of the unicycle is that, given an initial posture and 
a consistent desired output trajectory {xd{t),yd{t)) together with its deriva- 
tives, there is a unique associated state trajectory qd{t) = {xd{t),yd{t),Qd{t)) 
which can be computed in a purely algebraic way^, since 

9d{t) = kTiAJ^2{yd{t),Xd{t)) +kT: fc = 0, 1, (5.4) 

where the value of k is chosen so that 0d(O) = 0(0). If fc = 1, a backward 
motion will result. Therefore, the nominal orientation 9d{t) may be computed 
off-line and used for defining a state trajectory error. 

Note the following facts. 

— When the desired linear velocity Vd{t) is zero for some t, neither the nom- 
inal angular velocity nor the nominal orientation are defined from (5.3) 
and (5.4), respectively. This may occur at the initial instant, if a smooth 
start is specified, or at a cusp along the geometric path underlying the 

^ This is related to the fact that {x, y) is a flat output for the unicycle [15] or, 
equivalently, a linearising output under dynamic feedback (see Section 5.4). 
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Cartesian trajectory {xd{t),yd{tj). For the first case, one can use (if avail- 
able) higher-order differential information about {xd{t) , yd{t)) at t = 0 in 
order to determine the consistent initial orientation and the initial angular 
velocity command. For the second case, a continuous motion is guaran- 
teed by keeping the same orientation attained at t~; by using de L’Hopital 
analysis in (5.3), one can also compute the value of Udit). 

— More in general, the reference trajectory may be specified by separating the 
geometric aspects of the path (parameterised by a scalar s) from the timing 
law s = sit) used for path execution. The driftless nature of the kinematic 
model of a WMR allows to overcome in this way the above ‘zero velocity’ 
problem. For the unicycle, we can rewrite purely geometric relationships 
as 

dx/ds cos 6 0 

dy/ds = sin6* v' + 0 w' , 

dO/ds 0 1 

where the time commands are recovered as 

v{t) = v'{s)s[t) to{t) = Lo'{s)s{t). 

Zero-velocity points with well-defined geometric tangent (e.g. cusps) are 
then obtained for s{t) = 0. The feedforward pseudo-velocity commands 
u(j(s) and oj'd(s) are computed by replacing in (5.2) and (5.3) time deriva- 
tives with space derivatives. 



5.2 Linear Control Design 

The simplest trajectory tracking control design is based on tangent lineari- 
sation along the reference trajectory. Following [9], it is worth to reconsider 
the linearisation procedure of the unicycle around the trajectory. Define the 
state tracking error e as 

Cl cos 9 sin 0 0 Xd — x 

62 = — sin0 cos 9 0 yd — y ■ (5.5) 

.eaj [ 0 0 I \ [dd- 9 _ 

The difference between e and qu of Section 2 is in the rotation matrix, which is 
computed here at the current orientation, and in a change of sign in the right 
hand side. Using the following nonlinear transformation of velocity inputs 

V = Vd cos 63 - U1 , „ 

U! = COd-U2, ^ ^ 

the error dynamics becomes 
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(5.7) 
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Linearising (5.7) around the reference trajectory, we obtain the same linear 
time-varying equations (3.6), now with state e and input (ui,U 2 ). 

Define the linear feedback law 



Ui = —ki 6i 

U2 = -k2 sign(wd(t)) 62 - ks 63. 



(5.8) 



A desired closed-loop polynomial 

(A -f 2^a)(A^ -I- 2^aA + a^) 



namely having constant eigenvalues (one negative real at —2(^a and a complex 
pair with natural angular frequency a > 0 and damping coefficient C G (0, 1)) 
can be obtained by choosing the gains in (5.8) as 



k\ = kz = 2C,a 



_ - uJd{tY 

\Mt)\ 



However, ^2 will go to infinity (i.e. an infinite control effort would be required 
for the same transient performance) as Vd — > 0. Therefore, a convenient gain 
scheduling is achieved by letting a = a{t) = y/uj'j{t) + bv'j{t) so that 



ki^ ks = 2(^Juj{t) +bvj{t) k 2 = b\vd{t)\, (5.9) 

where the factor 5 > 0 has been introduced as an additional degree of freedom. 
According to the controllability analysis in Section 2, these gains gracefully go 
to zero when local controllability around the (state) trajectory is lost because 
the latter stops. 

In terms of the original control inputs, this design leads to the nonlinear 
time-varying controller 



V 

U! 



Vd cos{6d — 0) ki [cos 9{xd — 
u)d + k 2 sign(wd) [cos6{yd - y) 



x) +sme{yd - y)] 

(5.10) 

- sm9{xd - a;)] -I- ks{9d - 9). 



It should be emphasised that, even if the closed-loop eigenvalues are con- 
stant and with negative real part, this control law does not guarantee the 
asymptotic stability of the state tracking error e, because the system is still 
time-varying. A complete Lyapunov-based stability analysis can be however 
carried out by including a simple nonlinear modification, as shown hereafter. 



5.3 Nonlinear Control Design 

Following [26], we present now a nonlinear design for trajectory tracking. 
Consider again (5.7). Define 

ui = -ki{vd{t),ujd{t))ei 

U2 = -h Vd{t) ^^e^^ 62 - k3{Vd(t),U}d{t)) 63, 



(5.11) 




196 A. De Luca, G. Oriolo, M. Vendittelli 



with constant fc 2 > 0 and positive, continuous gain functions fci(-,-) and 

ksi;-). 

Theorem 5.1. Assuming that Vd and Ud are bounded with bounded deriva- 
tives, and that Vd{t) 7 ^ 0 or uJd{t) 7 ^ 0 when t — > 00 , the control law (5.11) 
globally asymptotically stabilises the origin e = 0 . 

Proof. (Sketch) It is based on the use of the Lyapunov function 

C = |(d + e^) + f, 

whose time derivative along the solutions of the closed-loop system is nonin- 
creasing since 

V = —kik2e\ — ^363 < 0. 

Thus, ||e(t)|| is bounded, V{f) is uniformly continuous, and V{t) tends to 
some limit value. Using Barbalat lemma, V it) tends to zero. From this and 
analyzing the system equations, one can show that -(- {i = 1, 2, 3) 

tends to zero so that, from the persistency of the (state) trajectory, the result 
follows. ■ 



Merging (5.5), (5.6), and (5.11), the resulting control law is 
V = Vd cos{6d -9) -i-ki [vd, ojd) [cos 9{xd - a;) -I- sin 0(j/d - y)] 



U) = Wd k2Vd 



sin{9d - 9) 
9d-9 



[cos 9 {yd - y)-s\n9{xd 



(5.12) 

a;)] + ks{vd,w>d){9d - 9). 



Taking advantage of the previous linear analysis, we can choose the gain 
functions k\ and k^ and the constant gain k 2 as 



ki{vd{t),Wd{t)) = ks{vd{t),uJd{t)) = 2()^wl{t) + hvlft) ^2 = b, 
with 5 > 0 and ( G (0, 1). 



5.4 Dynamic Feedback Linearisation 

A nonlinear controller for trajectory tracking based on exact dynamic feed- 
back linearisation is now designed following [11], [12]. 

With reference to the general class of nonholonomic driftless systems (3.2), 
the dynamic feedback linearisation problem consists in hnding, if possible, a 
dynamic state feedback compensator of the form 

i = a{q,0 +b{<l,Ou /g 

w = c{q,£,) -i-d{q,^)u, 

with i^-dimensional state ^ and m-dimensional external input u, such that the 
closed-loop system (3.2)-(5.13) is equivalent, under a state transformation 
2 : = T{q,^), to a linear controllable system. 
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Only necessary or sufficient (but no necessary and sufficient) conditions 
exist for the solution of the dynamic feedback linearisation problem. Con- 
structive algorithms, which are essentially based on input-output decoupling, 
can be found in [ 16 ]. 

The starting point is the definition of an appropriate m-dimensional sys- 
tem output 77 = h{q), to which a desired behaviour can be assigned (in our 
case, track a desired trajectory). One then proceeds by successively differ- 
entiating the output until the input appears in a nonsingular way. At some 
stage, the addition of integrators on a subset of the input channels may 
be necessary in order to avoid subsequent differentiation of the original in- 
puts. This dynamic extension algorithm builds up the state ^ of the dynamic 
compensator ( 5 . 13 ). The algorithm terminates after a finite number of differ- 
entiations whenever the system is invertible from the chosen output. If the 
sum of the output differentiation orders equals the dimension n + v oi the 
extended state space, full input-state-output linearisation is also obtained. 
The closed-loop system is then equivalent to a set of decoupled input-output 
chains of integrators from Ui to rji (f = 1, . . . , m). 

We illustrate this exact linearisation procedure for the unicycle model ( 3 . 3 ). 
Define the linearising output vector as 77 = {x,y). Differentiation w.r.t. time 
then yields 



X 




cos 9 


0 ■ 




V 


y . 




sin 9 


0 




CO 



showing that only v affects f], while the angular velocity (X cannot be recovered 
from this first-order differential information. In order to proceed, we need 
therefore to add an integrator (whose state is denoted by ^) on the linear 
velocity input 



v = ^ i = a 






cosO 
sin 9 



being the new input a the linear aceeleration of the unicycle. Differentiating 
further 



v = i 



cos 9 




— sin0 




cos 9 —^sin9 


a 


sin 9 




cos 9 


— 


sin 9 ^ cos 9 


CO 



and the matrix multiplying the modified input (a, u>) is nonsingular provided 
that C 7^ 0 . Under this assumption, we can define 



a 




cos 9 


—^sin9 


-1 


Ui 


CO 




sin 9 


^cos9 




_“2_ 



so as to obtain 



m 




Ml 


92 _ 




_“ 2 _ 



( 5 . 14 ) 
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The resulting dynamic compensator is 

^ = Ml cos 9 + U2 sin 9 

M = C (5-15) 



M2 cos 0 — Ml sin 9 




Since the dynamic compensator is one-dimensional, we have n+v — 3-1-1 = 4, 
equal to the total number of output differentiations in (5.14). Therefore, in 
the new coordinates 

Zi = X 



Z 2 = y 

Z3 = X = ^ cos 9 

Z4 = y = Csin6», 



(5.16) 



the extended system is fully linearised in a controllable form and described 
by the two chains of second-order input-output integrators given by (5.14), 
rewritten as 



i-i = Ml 
Z2 = M2. 



(5.17) 



Note that the dynamic feedback linearising controller (5.15) has a po- 
tential singularity at ^ = u = 0, i.e. when the unicycle is not rolling. The 
occurrence of such singularity in the dynamic extension process has been 
shown to be structural for nonholonomic systems [12]. This difficulty must 
be obviously taken into account when designing control laws on the equivalent 
linear model. 

Assume the robot must follow a smooth output trajectory {xd{t) , yd{t)) 
which is persistent, i.e. such that the nominal control input Vd = (i^ 
along the trajectory does never go to zero. On the equivalent linear and 
decoupled system (5.17), it is straightforward to design a globally exponen- 
tially stabilizing feedback for the desired trajectory (with linear Cartesian 
transients) as 



Ml = Xd{t) + kpi{xd{t) - x) + kdi{xd{t) - x) 
U2 = yd{t) + kp2{yd{t) - y) + kd2 {yd{t)-y), 



with PD gains chosen as kpt > 0, kdi > 0, for i = 1, 2. 

In the implementation, velocities x and y can be computed via the last 
two expressions in (5.16), as a function of the robot state and of the com- 
pensator state Alternatively, one can use estimates of x and y obtained 
from odometric measurements. This solution is more robust with respect to 
unmodelled dynamics. 

We conclude the discussion on trajectory tracking via dynamic feedback 
linearisation by offering some remarks: 
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— The state of the dynamic compensator should be correctly initialized at 
the value ^(0) = Vd{0). This guarantees exact trajectory tracking for a 
matched initial state of the robot. In this case, the control law (5.15-5.18) 
reduces to the pure feedforward action. 

— Being based purely on an output tracking error definition, this method 
requires neither the explicit computation of 9d{t) nor the measure of the 
orientation angle 9{t). 

— Even for smooth persistent trajectories, problems may arise if the actual 
command v = ^ crosses zero during an initial transient. However, this 
situation can be avoided by suitably choosing the initial state of the dy- 
namic compensator. For example, a simple way to keep the actual com- 
mands bounded is to reset the state ^ whenever its value falls below a 
given threshold. This strategy results in an input command v with isolated 
discontinuities with respect to time. 



5.5 Experiments 

In the following we report experimental results of SuperMARIO following the 
eight-shaped reference trajectory of Figure 5.1, defined by 

Xd{t)= sin ^ yd{t)= sin ^ t G [0,T]. 

The trajectory starts from the origin with 0^(0) = tt/ 6 rad. A full cycle is 
completed in T = 27 t • 20 « 125 s. Note that the reference initial velocities 
are 

Ud(0) ~ 0.1118 m/s Wd(0) = 0 rad/s. 

In the first set of experiments, the robot configuration is initially matched 
with the desired reference trajectory (i.e. with initial state q{0) = <?d(0)). 
Therefore, the feedforward commands (5.2), (5.3) would allow exact trajec- 
tory following in ideal conditions. However, if the unicycle starts at rest and 
non-zero high-level commands Vd{0) and Wd(0) are sent to the robot, due 
to the actuation/vehicle dynamics there will be some transient before these 
velocities are actually achieved at the physical low level. The addition of a 
feedback action to the feedforward command is also effective in recovering 
the induced state error. 

Figures 5. 2-5. 4 show the results obtained with the linearly designed con- 
troller (5.10), using the gains (5.9) with C = 0.7 and b = 10. The tracking of 
the reference trajectory of Figure 5.1 is indeed quite accurate. Residual errors 
are mainly due to quantisation and discretisation of velocity commands, as 
well as to other nonidealities. In particular, there is a large transient error 
due to the vehicle/actuator dynamics because of the initial non-zero value of 
Ud(O). This is clearly shown in Figure 5.5 which shows the norm |jep|| of the 
Cartesian error. 
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Figure 5.1. An eight-shaped reference trajectory 



Similar performance is obtained with the nonlinear controller (5.12), 
using the same gains as above, and with the dynamic feedback linearisa- 
tion controller (5.15), choosing the PD gains in (5.18) as kpi = kp 2 = 1, 
kdi = kd 2 =0.7 and initializing the dynamic compensator at ^(0) = Ud(0). 
To appreciate the slight improvement in performance, compare the norm ||ej,|| 
of the Cartesian error in Figures 5.6 and 5.7 with the previous result in Fig- 
ure 5.5. It is found that the average error is reduced from a value of 1 cm 
(linear design) to 0.5 cm (nonlinear design) and hnally to 0.38 cm (dynamic 
feedback linearisation design). 

A second set of experiments was performed letting q{0) = (0.2, —0.3, 7t/3) 
(m,m,rad), i.e. starting with an initial state error with respect to the assigned 
trajectory. Only the linearly designed controller and the dynamic feedback 
controller were compared (see Figures 5.8-5.11), using the same previous 
settings of control parameters. The obtained transients are quite similar, 
although a smaller overshoot is experienced with dynamic feedback lineari- 
sation, as implied by the choice of the PD gains. 
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Figure 5.2. Trajectory tracking with linear feedback design: x (— 
and 6 ( — ) (rad) vs. time (s) 



), y (— ) (m), 




Figure 5.3. Trajectory tracking with linear feedback design: driving velocity v 
(m/s) 




Figure 5.4. Trajectory tracking with linear feedback design: steering velocity u> 
(rad/s) 
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Figure 5.5. Trajectory tracking with linear feedback design: norm of Cartesian 
error (m) 




Figure 5.6. Trajectory tracking with nonlinear feedback design: norm of Cartesian 
error (m) 
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Figure 5.7. Trajectory tracking via dynamic feedback linearisation: norm of Carte- 
sian error (m) 
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Figure 5.8. Asymptotic trajectory tracking with linear feedback design: Cartesian 
motion (x, y) (m) 




Figure 5.9. Asymptotic trajectory tracking with linear feedback design: Cartesian 
errors Cx and Cy (m) 
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Figure 5.10. Asymptotic trajectory tracking via dynamic feedback linearisation: 
Cartesian motion {x, y) (m) 




Figure 5.11. Asymptotic trajectory tracking via dynamic feedback linearisation: 
Cartesian errors 6x and 6y (m) 








204 A. De Luca, G. Oriolo, M. Vendittelli 

6 Posture Stabilisation 

As mentioned in Section 3, posture stabilisation for nonholonomic WMRs 
cannot be achieved by smooth static feedback. We present two controllers 
based on time-varying feedback and two based on discontinuous feedback. 

6.1 Smooth Time-varying Control 

It has been shown in Section 5.3 that asymptotic stabilisation of a state track- 
ing error can be achieved provided that Vd{t) and ujd{t) — which introduce 
a time-varying signal in the feedback control law — do not both vanish in 
finite time. This observation suggests that a solution to the posture stabilisa- 
tion problem can be obtained by designing a desired motion, to be used as a 
fictitious time- varying reference, which asymptotically vanishes at the origin. 

Following [26], the structure of a smooth time- varying stabilizing con- 
troller is the same of the nonlinear trajectory tracking controller (5.11): 

ui = -ki{vd{t),ujd{t))ei 

( G 1 ^ 

U2 = -k2 62 - ks^Vdit), UJd{t)) 63, 

with the same notation used in Section 5.3 (in particular, (5.12) is used to 
generate v and u). In this case, however, the desired trajectory is itself an ad- 
ditional degree of freedom, subject to the constraint that, under control (6.1), 
both the state tracking error e and the desired trajectory are asymptotically 
stabilised to the origin. A simple solution is to set, for all times t, yd{t) = 0 
and 9d{t) = 0 (and thus Ud{t) = 0), having only Xd in motion. A class of 
desired velocities is given then by 

Vd{t) = Xd{t) = -k^Xd{t) + g{e, t) k^ > 0 , ( 6 . 2 ) 

where g{e, t) is a C^-function uniformly bounded with respect to t, together 
with its partial derivative, and such that: 

Al. g{0,t) = 0, for all t; 

A2. there exists a diverging sequence of instants {ti}i^N and a continuous 
function o:(-) for which 

||e||>£>0 ^ ^||(e,t*)^ > a(e) > 0 Vti. 

Theorem 6.1. The smooth time-varying controller (6.1), with Vd{t) given 
by (6.2) with Al and A2, globally asymptotically stabilises e = 0 and ay = 0. 

Proof. (Sketch) Using the same Lyapunov function as in Theorem 5.1, the 
tracking error e{t) is shown to be bounded. Therefore, (6.2) is a stable linear 
system subject to an additive bounded perturbation g{e, t), so that Xd{t) and 
Vd{t) (as well as Vd{t)) remain bounded. Theorem 5.1 applies and the rest of 
the proof uses assumptions Al and A2 to show first that Vd{t) tends to zero, 
then that also ||(e,t) does, and concluding the convergence of e to zero. 
Finally, using A2, (6.2) implies that also Xd{t) tends to zero. ■ 
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The heating function g{e,t) plays a key role in guaranteeing asymptotic 
stability by sustaining motion as long as the error e is not zero, but it also 
determines the transient behaviour. Possible choices satisfying A1 and A2 
include g{e,t) = ||e|psint and, if the functions ki{-, •) and fc 3 (-, •) are strictly 
positive. 



g{e,t) 



exp(fc5e2) - 1 
exp(fcse2) + 1 



^5 > 0 . 



(6.3) 



We tested the performance of the time- varying control (6.1), with desired 
motion given by (6.2), initialised at a;d(0) = 0, and heating function (6.3). 
The gains have been selected as ki = 0.5, = 2, fca = 1, k^ = 1, and k^ = 50. 

A forward parking task from g(0) = (— 1,— 1,0) (m,m,rad) to the ori- 
gin is assigned. This task will be used as a baseline for comparison of all 
posture stabilisation methods. Figures 6. 1-6.4 show that, after a relatively 
fast approach, convergence of the smooth time-varying controller becomes 
extremely slow when the unicycle is close to the goal. In particular, this is 
evident in Figure 6.2, a stroboscopic view of the robot motion sampled ev- 
ery 10 s. An inherent limitation of this control design is the large number of 
backup maneuvers, executed with the unicycle approximately aligned with 
the final desired orientation. The oscillation range of x is contained in the 
range (—1, 1), thanks to the large value of k^. The steering velocity command 
becomes very small in the final phase. 

The accuracy in regulation to the origin is determined by the satisfac- 
tion of the following terminal bounds, used in all the posture stabilisation 
experiments: 



|a;| < 0.2 cm |j/| < 0.2 cm \9\ < 0.02 rad. 



/j| 






.■ i' , 







’^0 20 40 60 80 100 120 140 160 180 200 



Figure 6.1. Posture stabilisation with smooth time-varying feedback: x ( ), 

( — ) (m), and 9 ( — ) (rad) vs. time (s) 



y 
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Figure 6.2. Posture stabilisation with smooth time-varying feedback: Cartesian 
motion {x, y) (m) 




Figure 6.3. Posture stabilisation with smooth time-varying feedback: driving ve- 
locity V (m/s) 




Figure 6.4. Posture stabilisation with smooth time-varying feedback: steering ve- 
locity to (rad/s) 
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6.2 Nonsmooth Time-varying Control 



By giving up the smoothness requirement, several controllers have been pro- 
posed for posture stabilisation with improved transient performance. We re- 
view here one of the first such designs [32], which applies to nonholonomic 
systems that can be transformed into chained form. The control law is con- 
tinuous in time but nonsmooth with respect to the state, which is fed back 
only at uniformly sampled instants. 

Consider the chained-form (3.9) as an equivalent unicycle model, and 
note that the origin of the (x, y, 0) configuration space (which is the desired 
posture qd) maps into the origin of the (.21,22,^3) space. If the input Ui is 
a predefined function of time, Z23 = [22 23]^ satisfies a linear time-varying 
equation driven by the input U2 ■ The command ui is obtained by combining 
a simple open-loop command, which is updated as a function of the state 
only on a discrete-time basis, with a time-varying exogenous signal, in such 
a way that 2i converges exponentially to zero when \\z23W does. The other 
input U2 is chosen so that H223II converges to zero with an exponential rate. 

In particular, let a sequence of uniformly spaced instants {to,ti,t2, ■ ■ ■} 
be defined as 

th = hT T = th+i — th > 0- 
Define the control Ui as 



ui{t) = k{z{th))f{t) for t e [th,th+i). (6.4) 

This input is a function of the state 2 at time t = th, while during the 
interval (th,th+i) it is defined in an open- loop fashion. Choosing a smooth 
and periodic function f{t), such as 

1- cosuit 2 tt 

f{t) = 2 

ui{t) is guaranteed to be continuous in time. The control function k{z(th)) 
is given by 

k{z{th)) = -P [ziith) + sgn{zi{th)) j{\\z23{th)\\)] , (6.6) 



where 



and 



sgn(2i) = 



1 , 

- 1 , 



if 2i > 0, 
if 2i < 0, 



= 7 * 77777 ^ = T 7(lk23||) = k||223|1^ «>0. 

it, fi^)dr ^ 

The control law for U2 is designed based on the backstepping principle [30] . 
Assume that the variable 22 in the third equation of the chained form 



23 = Z 2 Ui{t) 



(6.7) 
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is a ‘dummy’ control input In order to stabilise this subsystem, we choose 



2 



d 

2 



Ui{t) 



As 

k{z{th)) 



f{t)Z3 



As > 0. 



( 6 . 8 ) 



It can be shown that z$ exponentially converges to zero with a rate depending 
on A3. In the presence of a transient difference Z2 = Z2 — Z2, one can also prove 
that Z3 exponentially converges to zero if S2 does. Therefore, U2 should be 
designed so as to make Z2 converge to enforcing thus the desired behaviour 
for Z3. Since the second equation of the chained form is 



2:2 = W 2 , 

this is accomplished, with exponential convergence, by choosing 

U2 = -X2{Z2 - Z 2 ) + Z 2 A 2 > 0, 

where, from (6.8), 

= (2/(f)/(f)-^^ + />(^, 

in which we have used (6.4), (6.7), and the fact that k{z{th)) is constant over 
{th : th+l') • 

The complete nonsmooth time-varying controller is then 
ui = -k{z{th))f{t) 

U2 = -{X2+X3f{t))Z2-X3{X2p{t)+2f{t)f{t))-^jj^. 

Equation (6.9) should be used in conjunction with (3.8) in order to generate 
the actual velocity inputs v and iv. 

Defining a class /C function as a strictly increasing function h : 1-^ 

such that h{0) — 0, the main result is summarised in the following theorem. 



Theorem 6.2. Consider the unicycle in chained form (3.9) under the con- 
trol (6.9), with the definitions (6.5) and (6.6). Then, the origin z = Q is 
JC- exponentially stable, i.e. there exist a constant Az > 0 and a class K. func- 
tion hz{-,T) such that 

\\z{t)\\ < hz{\\z{0)\\,T) V2(0) G > 0. 



Proof. It is a specialisation of the general proof for n-dimensional chained- 
form systems, see [32]. ■ 

Figures 6. 5-6. 8 show the results of the application of the control law (6.9), 
with T = 6 s and k = 0.8, A2 = A3 = 0.4, for executing the baseline forward 
parking task. The rate of convergence of the nonsmooth time-varying con- 
troller is somewhat improved but still quite slow. A stroboscopic view of the 
unicycle motion sampled every 5 s is reported in Figure 6.6. Note that the 
approach in the y direction is very uniform, while maneuvers in the vicinity 
of the goal are aimed at adjusting 9 rather than x. This is intrinsic in the 
structure of the chained form used for the control design. 
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Figure 6.5. Posture stabilisation with nonsmooth time- varying feedback: x ( ), 

y ( — ) (m), and 6 ( — ) (rad) vs. time (s) 




Figure 6.6. Posture stabilisation with nonsmooth time- varying feedback: Cartesian 
motion (x, y) (m) 




Figure 6.7. Posture stabilisation with nonsmooth time-varying feedback: driving 
velocity v (m/s) 
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Figure 6.8. Posture stabilisation with nonsmooth time-varying feedback: steering 
velocity to (rad/s) 



6.3 Control Based on Polar Coordinates 



Another technique which allows to overcome the obstruction of Brockett the- 
orem is to apply a change of coordinates such that the input vector fields 
of the transformed equations are singular at the origin. This approach has 
been proposed in [1], where a Lyapunov-like design of a posture stabilizing 
controller is carried out using a polar coordinate transformation. The control 
law, once rewritten in terms of the original state variables, is discontinuous 
at the origin of the configuration space Q. 

With reference to Figure 6.9, we define the following set of polar coordi- 
nates for the unicycle. Let p be the distance of the reference point {x, y) of 
the unicycle from the goal (the origin), 7 be the angle of the pointing vector 
to the goal w.r.t. the unicycle main axis, and 6 be the angle of the same 
pointing vector w.r.t. the x axis (orientation error), i.e. 

p = \/x‘^ + y‘^ 

7 = ATAN2(y, a;) — 0 + 7T 
b = 7 - 1 - 0 . 

Angles 7 and b are undefined for a: = y = 0. In practice, during the vehicle 
motion, when p — > 0 one must retain the values for 7 and b assumed in the 
final approaching phase. In these coordinates, the unicycle equations become 



p = — cos 7 V 

sin 7 

7 = V — to 

P 

■ sin 7 

b = V. 

P 

The input vector field associated to v is singular for p = 0. 



(6.10) 
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y 



X 



Figure 6.9. Definition of polar coordinates for the unicycle 




In order to achieve the goal posture, variables e, 7, and 6 should all 
converge to zero. This is guaranteed by the following result, adapted from [1], 

Theorem 6.3. Consider the polar coordinate description (6.10) of the uni- 
cycle and the feedback control 

V = fcipcosy 

, , , sin 7 cos 7 / , 7 r\ (6-11) 

U) = / c 27 + «i ^ -h + ho), 

with k\ and k 2 positive constants. The closed-loop system (6.10)-(6.11) is 
then globally asymptotically driven to the posture = (0,0,0). 

Proof. (Sketch) Consider the function 

V = i + 7^ + , 

as a candidate Lyapunov function. Its time derivative along the solutions of 
the closed-loop system is nonincreasing since 

V = —ki cos^7p^ — ^27^ < 0. 

Thus, the state is bounded in norm, V (t) is uniformly continuous, and V (t) 
tends to a limit value. By Barbalat lemma, V (t) tends to zero and thus also 
p and 7 do. Analyzing the closed-loop system, one can show that p and 6 
converge to zero, that 6 converges to some finite limit 6 while 7 tends to 
the finite limit —kik^h and is uniformly continuous. Again through Barbalat 
lemma, this finite limit must be zero and thus also b converges to zero. ■ 

Figures 6.10-6.13 refer to the results of the application of controller (6.11), 
with gains = 1, /c2 = 3 and fcs = 2, for executing the baseline forward park- 
ing task. The convergence to the goal is very fast and natural. In Figure 6.11, 
a stroboscopic view of the unicycle motion sampled every 1 s is given. Both 
velocity commands are saturated for 2 to 3 seconds. 
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In the second experiment, a parallel parking task from g(0) = (0, —1.5, 0) 
(m,m,rad) to the origin of the configuration space is assigned. The same 
gains as in the previous experiment have been used. The obtained results are 
shown in Figures 6.14-6.17 and indicate that there is no backup maneuver 
in this case. If the robot had been initially closer to the positive x axis, 
the controller (6.11) would have automatically driven the robot backwards 
and then in forward motion to the goal. This is a general property of the 
controller: in the final phase, the vehicle will always approach the goal in 
forward motion, having executed at most one backup maneuver. 

We finally note that the behaviour of the controlled system is not contin- 
uous with respect to the initial state. For example, assume that the initial 
configuration is x(0) = a > 0, y{0) = e and 0(0) = 0. Positive and negative 
arbitrarily small values of e will lead to different transient motions to the 
goal (in fact, symmetric with respect to the x axis). 




Figure 6.10. Posture stabilisation using feedback in polar coordinates (forward 
parking): x ( ), y ( — ) (m), and 9 ( — ) (rad) vs. time (s) 




Figure 6.11. Posture stabilisation using feedback in polar coordinates (forward 
parking): Cartesian motion (x,y) (m) 
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Figure 6.12. Posture stabilisation using feedback in polar coordinates (forward 
parking): driving velocity v (m/s) 




Figure 6.13. Posture stabilisation using feedback in polar coordinates (forward 
parking): steering velocity cj (rad/s) 




Figure 6.14. Posture stabilisation using feedback in polar coordinates (parallel 
parking): x ( ), y ( — ) (m), and 0 ( — ) (rad) vs. time (s) 
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Figure 6.15. Posture stabilisation using feedback in polar coordinates (parallel 
parking): Cartesian motion {x,y) (m) 




Figure 6.16. Posture stabilisation using feedback in polar coordinates (parallel 
parking): driving velocity v (m/s) 




Figure 6.17. Posture stabilisation using feedback in polar coordinates (parallel 
parking): steering velocity w (rad/s) 
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6.4 Dynamic Feedback Linearisation 

Following [14], we show how to extend the trajectory tracking controller based 
on dynamic feedback linearisation to address the posture stabilisation prob- 
lem, while avoiding the intrinsic singularity that occurs when the robot comes 
to a stop. This simply requires an appropriate choice of the PD gains and a 
suitable initialisation of the dynamic compensator state 
We denote by 

Q* = {q € Q : {x = 0, cos'd > 0) OR {y = 0, cosd = —1)} 

a subset of Q which will require special attention. The remaining part QjQ* 
of the conhguration space can be partitioned in two regions: 

S" = {(Z G Q/Q* : > 0} 

S' = {q&Q/Q* -.xkO}. 

The main result is the following. 

Theorem 6.4. Co'nsider the unicycle system (3.3) under the action of the 
dynamic compensator (5.15). A PD control law on the Cartesian error 

U\ — hp\X k(j[\X /g 

U2 = -kp2y - kd2y ' 

yields exponential convergence from any initial configuration q{0) € Q/Q" to 
the origin, if the following assumptions hold: 

Al. The control gains kpi > 0, kdi> (i = \,2) satisfy the conditions 



^dl 4/ppi — k^2 ^ 0 


(6.13) 


kd2 - kdi > 2 J - 4/cp2- 


(6.14) 



A2. The initial state of the dynamic compensator is chosen as 

C(0) < 0 (backward motion) if q(0) G Of 

f{0) > 0 (forward motion) if q{0) G S', 

but its value is otherwise arbitrary, except for the additional condition 

sin ^'(0) - kp 2 y{ 0 ) cos 0(0) 

r ^ r r 

kd2 — kdi 



(6.15) 
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Proof. Use of control (6.12) in (5.17) implies that the Cartesian coordinates 
X and y converge to zero exponentially, provided that the original control 
inputs V and w given in (5.15) remain bounded. To show this, we must prove 
that (i) ^ does not go to zero in hnite time, and (ii) uj tends to zero for 
t — > 00 , in spite of its denominator ^ vanishing. 

(i) Since from (5.16) it is = z^+zl , one has ^(t) = 0 iff Zsit) = Zi{i) = 

0, for a generic instant t > 0. Integrating the closed-loop system (5.17) under 
control (6.12), we have 

Zi{t) = a3ie^“* + 0326^1^* (6.16) 

z^{t) = 0416^=^* -f 0426^^^*, (6.17) 

where coefficients Okj and eigenvalues A^- are easily obtained as functions of 
initial state and PD gains. From these expressions and condition (6.13), it is 
possible to show that a finite t > 0 such that ^(t ) =0 exists iff 

^(0) cos 0(0) — a;(0)Aii ^(0) sin 0(0) — y(0)A2i 

^(0) cos 0(0) - a;( 0 )Ai 2 ^(0) sin 0(0) - y( 0 )A 22 ’ 

with a = A 11 A 22 /A 12 A 21 . From this, a quadratic equation in ^(0) is derived 
which has the single nonzero root 

, . _ Aiia;(0) sin0(O) -I- A22?/(0) cos0(O)-a(Ai2x(O) sin0(O) -f A2ij/(0) cos0(O)) 

(1 — a) sin 0(0) cos 0(0) 

Once rewritten in terms of the PD gains, this expression leads to the forbidden 
initialisation condition (6.15). 

(ii) First of all, it is straightforward to prove that assumption A1 implies 
that the eigenvalues are real and ordered as An < A 12 < A 21 < A 22 < 0. 
From (5.15), we rewrite u) as 



-ZaUi + Z3U2 




and using (6.16-6.17) and (6.12), the numerator of 00 takes the form 
-Z4U1 + Z3U2 = ^^^{\ll+\22)t ^^^(\l2+\22)t ^ 

with 7 i G M. Its asymptotic rate of convergence is certainly larger than 
2 IA 22 I due to the eigenvalue ordering. As for the denominator, squaring and 
adding (6.16) and (6.17) gives 

^2 ^ -f 7726^^“+^^"^* -f 7736^^'"* -f 7746^^"'* -f 

with r]i G M. Since the asymptotic rate of convergence of this quantity is 
exactly 2 IA 22 I, we conclude that u tends to zero as t ^ 00 . 

To finish the proof, it is necessary to show that also the orientation 0 
converges to zero. This is easily understood from the following facts: 
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— The unicycle reaches the origin with a horizontal tangent (0 = 0 or tt), 
because y approaches zero faster than x in view of the eigenvalue ordering. 

— Motion inversions do not occur since v = ^ never crosses zero, as shown in 
the first part of the proof. 

— The trajectory is confined to the region (either Qr or Qi) from which the 
unicycle starts. In fact, x and y never change sign because the eigenvalues 
are real and thanks to the choice of sign for ^(0) in assumption A2. 

Finally, also the convergence of 9 to zero is exponential. Indeed, since oo goes 
exponentially to zero, the same is true for its integral 9. ■ 

Some remarks are needed at this point. 

— As the Cartesian position transients are linear, the unicycle trajectories 
obtained with the proposed controller are completely predictable and can 
be easily shaped by choosing the PD control gains. Note that the unicycle 
can reach the goal either with a forward or with a backward motion. 

— The equality part of condition (6.13) in Theorem 6.4 is by no means nec- 
essary; it is only used for deriving a closed form for the forbidden initiali- 
sation (6.15) of the dynamic compensator. 

— In view of the discontinuity at the origin of the linearising controller with 
respect to the state {x,y,9,^) of the extended system, as well as of the 
fact that the initial configuration should belong to QjQ*, the proposed 
feedback controller does not yield Lyapunov stability in a strict sense, but 
simply exponential convergence. 

If the initial configuration q{0) belongs to Q* , Theorem 6.4 cannot be 
applied. In fact, the PD control (6.12) would bring the unicycle to the origin 
with the wrong orientation, namely, 0 = tt if cos 0(0) > 0, 0 = ±7t/2 if 
cos 0(0) = 0, 0 = — 7T if cos 0(0) = —1. In such a situation, it is necessary to 
reset the compensator state at some time ty > 0, so as to invert the motion 
at a configuration q G QjQ* . A simple way to obtain this is to introduce a 
via point q^ in the regulation procedure, as illustrated later in this section by 
the parallel parking experiment. This should not be seen as a drawback of the 
method; a suitable choice of the via point allows better control of the path 
shape while approaching the goal configuration. In particular, the resulting 
stabilisation motion contains at most one backup maneuver. 

Figures 6.18-6.21 refer to the baseline forward parking task, using dy- 
namic feedback linearisation plus PD control with gains fcpi = 2, kdi = 3, 
kp 2 = 12, kd 2 = 7, and compensator initialisation ^(0) = Vmax = 0.3 (m/s). 
The convergence to the goal is fast and very natural, as shown in Figure 6.19, 
a stroboscopic view of the robot motion sampled every 1.5 s. Note that sat- 
uration occurs on both inputs during the transient phase. 
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Figure 6.18. Posture stabilisation using dynamic feedback linearisation (forward 
parking): x ( ), y ( — ) (m), and 6 ( — ) (rad) vs. time (s) 




Figure 6.19. Posture stabilisation via dynamic feedback linearisation (forward 
parking): Cartesian motion (x,y) (m) 




Figure 6.20. Posture stabilisation via dynamic feedback linearisation (forward 
parking): driving velocity v (m/s) 
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Figure 6.21. Posture stabilisation via dynamic feedback linearisation (forward 
parking): steering velocity cj (rad/s) 



We also report the results for a parallel parking task with initial con- 
figuration given by ( 7 ( 0 ) = (0, — 1,0) (m,m,rad). The via point is chosen as 
Qy = (-1,-0. 6,0) (m,m,rad). The PD gains are the same as before, while 
the compensator state initialisation is chosen here as ^(0) = —Vmax for the 
first phase, performed in backward motion, and ^{ty) = Vmax for the second 
phase, which is started in a neighbourhood of qy and performed in forward 
motion. The results are shown in Figures 6.22-6.25. 

The simple first-order linear filter introduced to account for actuator dy- 
namics is also effective in smoothing the discontinuity in the driving velocity 
generated by the reset procedure. On the other hand, the presence of the 
same filter for the steering velocity, coupled with the software velocity satu- 
ration, neutralises the effect of the singularity in lo due to the zero crossing 
of the filtered driving velocity. 




Figure 6.22. Posture stabilisation via dynamic feedback linearisation (parallel 
parking): x ( ), y ( — ) (m), and 9 ( — ) (rad) vs. time (s) 
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Figure 6.23. Posture stabilisation via dynamic feedback linearisation (parallel 
parking): Cartesian motion {x,y) (m) 




Figure 6.24. Posture stabilisation via dynamic feedback linearisation (parallel 
parking): driving velocity v (m/s) 




Figure 6.25. Posture stabilisation via dynamic feedback linearisation (parallel 
parking): steering velocity w (rad/s) 
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7 Guidelines for End-users 

7.1 Summary and Comparison 

We have performed several motion tasks with SuperMARIO using the pre- 
sented control laws. The experimental tests presented in this chapter are 
representative of the average performance of the controllers. We summarise 
hereafter our acquired experience in general observations that can be useful 
guidelines for implementation of the same control strategies for other vehicles. 

First of all, the computational load for all methods is quite similar in the 
case of the unicycle. Basically, both trajectory tracking and posture stabili- 
sation controllers can be implemented with on-board computing power. Our 
choice of separating high-level control routines, performed on a remote server, 
from the low-level control loops in charge of realizing the reference velocity 
commands on each wheel reflects only the choice of a modular structure. It 
is however expected that such decomposition would become more convenient 
or even mandatory for wheeled mobile robots with more complex kinematics, 
such as a tractor vehicle towing a number of trailers. 

The three reviewed control methods for trajectory following tasks show 
similar performance. All of them can be generalised to more complex vehi- 
cles, provided their models are transformable in chained-form. Such gener- 
alisations can be found in [27] and [13]. From the point of view of control 
parameters tuning, especially for more complex WMRs, the dynamic feed- 
back linearisation technique promises to be simpler since it always boils down 
to the choice of stabilizing gains for a chain of integrators [13]; in particu- 
lar, it can be carried out on the original equations without resorting to the 
transformation in chained form. 

In Table 7.1, the control results for posture stabilisation tasks are com- 
pared in terms of performance, ease of control parameters tuning, sensitivity 
to nonidealities, generalisability to more complex nonholonomic WMRs, and 
relations with the design of tracking controllers. 

Time-varying controllers, both smooth and nonsmooth, exhibit a rather 
slow final convergence to the goal in spite of substantial progress during the 
first motion phase. In general, the nonsmooth controller should behave better 
because it achieves an exponential rate of convergence, but the dependence of 
this rate on the available gains is critical. The oscillatory behaviour of the ve- 
hicle during the approach to the goal, which makes the motion rather erratic, 
is an intrinsic characteristic of both time-varying control laws. In fact, the 
exogenous time dependence needed for stabilisation is introduced through the 
oscillatory motion of a virtual reference vehicle in the method of Section 6.1 
and through the periodic time function weighting the first command in the 
method of Section 6.2. In any case, the presence of several motion inversions 
makes these methods quite sensitive to mechanical nonidealities (e.g. back- 
lash) of the wheels. This situation may introduce a remarkable difference 
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Smooth 
time- varying 
stabilisation 


Nonsmooth 
time- varying 
stabilisation 


Design with 
polar 

coordinates 


Dynamic 

feedback 

linearisation 


Achieved 

performance 


very slow 
erratic 


slow 

erratic 


fast 

natural 


fast 

natural 


Ease of 
control tuning 


a few parameters 
problematic 


a few parameters 
critical 


simple 


simple PD 
^ initialisation 


Sensitivity to 
nonidealities 


many backups 


backups and 
sampled feedback 


good 


good 

integral action 


Generalisation 
to other WMRs 


yes if 

chained form 


yes if 

chained form 


no 


yes if 

chained form 


Relation with 
tracking control 


extended 
from tracking 


none 


none 


same PD 
control law 



Table 7.1. A comparison of the posture stabilisation controllers presented in this 
chapter 



between computed configuration from the internal odometry and actual dis- 
placement of the vehicle on the ground. In our experience, this behaviour was 
confirmed also in experiments performed with a car-like vehicle (the MARIO 
robot [25] ) , where a nonnegligible backlash on the steering angle of the front 
wheels led to a substantial error in the hnal positioning. Another potential 
problem with the presented nonsmooth controller is that, being based on a 
low-rate sampled state feedback (see (6.4)), the robot could in principle ‘miss’ 
the final goal even if passing through it. Among the positive features of these 
time-varying control laws, we mention that they can readily be generalised 
to more complex WMRs allowing a chained-form representation (see [27], 
[32]). Also, the smooth time- varying controller is a direct outgrowth of the 
trajectory tracking controller of Section 5.3. 

The controller based on polar coordinates transformation performed very 
well. The resulting vehicle path is very natural (in the sense that is similar 
to the one followed by an experienced human driver) and practical conver- 
gence is quite fast, with a weak dependence on the choice of the few control 
gain parameters. Since at most one backup maneuver is needed, disturbances 
due to wheel backlash are minimised. Unfortunately, a direct extension of 
such controller is not available for vehicles with more complex kinematics. 
The idea of using a state-space transformation that is singular at the goal 
configuration, however, stands on its own and has been exploited by other re- 
searchers, e.g. in [3]. Also, the polar coordinates controller has been adapted 
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in [1] for stabilisation about successive via points extracted from a desired 
path, but in this way only approximate trajectory tracking can be obtained. 

Similar positive comments can be drawn on the performance of the pos- 
ture stabilisation method designed via dynamic feedback linearisation. In 
particular, this scheme allows also parallel parking with backward/forward 
motion, which is a very natural maneuver. The control tuning requires the 
choice from a very large feasible set of PD gains. The relationship with the 
analogous controller for trajectory tracking is very simple: it is sufficient to 
add the feedforward terms, i.e. the reference output position, velocity and 
acceleration (compare (5.18) with (6.12)). As for the use of an additional dy- 
namics in the control law, it has some pros and cons. On one side, this design 
automatically takes into account the nonideality of a first-order kinematic 
model of the unicycle, by bringing linear acceleration into the picture. On the 
other side, it is necessary to prevent zeroing of the compensator state and 
the consequent singularity of the control commands; this may be achieved in 
practice by the simple strategy of filtering plus saturating the velocity com- 
mands. The generalisation to point-to-point motion tasks for WMRs with 
more complex kinematics is under way. It basically consists in extending the 
idea of suitably shaping the transient behaviour on the linear side of the prob- 
lem by appropriate selection of the gain structure (a for n generalised 

coordinates), so as to achieve a smooth and correct ‘entrance’ into the goal 
for the two outputs representing the robot Cartesian position. 

All the controllers reviewed in this chapter use a measure of the state 
reconstructed on the basis of the robot odometry. In principle, the actual 
motion of SuperMARIO on the ground may be quite different and should be 
computed with the aid of exteroceptive sensors. However, in our experiment, 
this difference could not be appreciated visually (as shown by the videos on 
the web page http : / /labrob . ing . uniromal . it/projects/ramsete .html). 
The satisfactory performance of our dead reckoning localisation system is of 
course related to the execution of relatively slow motion tasks. 

A hnal remark is needed about the application of the control methods re- 
viewed in this chapter when workspace obstacles are present. In a completely 
known environment, it may be convenient to tackle the navigation problem 
of a WMR using a three-layer control structure. The highest layer is devoted 
to motion planning and takes care of the nominal avoidance of obstacles; the 
nonholonomic motion constraints of the WMR may or may not be taken into 
account at this stage. The second layer takes charge of motion execution and 
uses one of the trajectory tracking controllers given in this chapter. In the 
vicinity of the goal, fine posture regulation (docking) can be obtained at the 
lowest layer by means of one of the presented stabilizing controllers. 

7.2 Future Directions 

There are some important issues barely mentioned in this chapter that de- 
serve further research. 
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Inclusion of dynamics. For massive vehicles and/or at high speeds, considera- 
tion of robot dynamics is necessary for realistic control design. The dynamics 
of general nonholonomic systems is thoroughly analysed in [5] and, more 
specifically for WMRs, in [7]. Interestingly, nonlinear static state feedback 
can be used to cancel, in the nominal case, all inertial parameters so as to 
lead to a purely (second-order) kinematic model of the form 

q = G{q)w, w = a, 

in place of (3.2), with {q, w) as the (n-l-m)-dimensional state and acceleration 
a as the new control input. The control laws used in this chapter do not 
directly apply to this case (they may have finite jumps in the velocity), but 
it is relatively easy to rework feasible modifications. 

Robust control design. Very few papers have explicitly addressed robustness 
issues in the control of nonholonomic systems. Robust stabilisation of WMRs 
in chained form was obtained in [4] and [19] by applying iteratively an open- 
loop command; exponential convergence to the desired equilibrium is ob- 
tained for small perturbations in the kinematic model. Another possible ap- 
proach to the design of effective control laws in the presence of nonidealities 
and uncertainties is represented by learning control, as shown in [25]. We 
also note that perturbations acting on nonholonomic mobile robots are not 
of equal importance: a deviation in a direction compatible with the vehicle 
mobility (e.g. slippage of the wheels on the ground) is clearly not as severe 
as a deviation which violates the kinematic constraints of the system (e.g. 
lateral sliding). 

Use of exteroceptive feedback. Proprioceptive sensors, such as incremental 
encoders, are obviously unreliable in the presence of wheel slippage. As a 
result, the robot may progressively ‘lose’ itself in the environment. A possi- 
ble solution is to close the feedback loop using exteroceptive sensors, which 
provide absolute information about the robot localisation in its workspace; 
sensor fusion techniques are relevant at this stage. The design of sensor-level 
controllers for nonholonomic robots is at the beginning stage but growing 
fast. An example of an on-board visual servoing for trajectory tracking is 
presented in [20]. 

Extension to WMRs not transformable in chained form. Among the open 
problems in motion control of general WMRs, we mention the case of multi- 
body vehicles that do not admit a transformation in chained form, such as 
a unicycle or car-like tractor with two or more trailers hooked at a non-zero 
distance from the axle of the previous moving body. A possible approach to 
posture stabilisation, using iterative steering of a nilpotent approximation 
model, can be found in [33]. 
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Topological graphs can be used as a world representation for a mobile robot 
that navigates in an office- like environment. Nodes of the graph can rep- 
resent the intrinsic structure of the environment like corridors, corners and 
so on. Arcs can capture the connectivity of the space. The task of building 
algorithms able to identify the characteristic features of nodes directly from 
sensory data requires several intermediate steps. They consist in the collection 
of the raw data, their reduction to a possibly one-dimensional representation, 
its “filtering” and, finally, the “feature recognition” . In the paper several of 
the many possible methodological choices are shown, with a priority to the 
(according to the authors) most advanced ones. 

In particular, for the “feature recognition” step, two different policies are 
shown: one founded on the use of a static case library that takes into account 
the previous results through the Transferable Belief Model, and a second one 
exploiting an architecture based on Case-Based Reasoning, a method from the 
Artificial Intelligence domain. Using the latter the robot acquires knowledge 
on a progressive basis and is therefore able to navigate autonomously in an 
environment without any prior information. 



1 Introduction 

Mobile robotics is on the borderline between Automatic Control and Artificial 
Intelligence. Scientists from the two communities use autonomous robots as 
a case study for their theories and to assess the validity of some specific tech- 
niques. As a matter of fact, both the approaches have their strong points and 
the problems found in mobile robotics are so wide in scope that everybody 
can find his own suitable situation. According to a very sketchy comparison, 
a control theory approach to navigation provides algorithms able to manage 
directly raw sensor data and to guarantee some nice convergence properties 
under suitable assumptions. On the other hand, a speculative approach can 
draw plans to tackle with complex environments and robot interactions. How- 
ever it needs symbolic information that generally is not available in real time 
and that must be derived from the measures by classification procedures. The 
chapter is intended to introduce several useful techniques, to scan some of the 
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existing literature, and to show in detail an experimentally verified approach 
that bridges this gap and tries to get the most from the two points of view. 

The chapter is based on a two-year long research work in the field; during 
this period several methodologies have been considered and tested, with the 
aim of using a more abstract representation of knowledge both in planning 
and in navigation. The overall navigation structure has been kept rather 
constant during the research and it is briefly described in Section 3. It is 
composed by several layers, each one can be implemented taking different 
approaches. 

In the chapter, a review of some advanced possibilities is referred for the 
layers. First, a short review of the main concepts about topological maps and 
fuzzy morphology is given. Then, Section 4 describes how to to build and 
enhance a 2-dimensional data structure (a map) based on rough measures. 
Section 6 shows a data reduction procedure to derive a one-dimensional repre- 
sentation of the space around the robot and, then. Section 8 details different 
approaches to associate a label to this representation, i.e. to recognise the 
kind of site where the robot currently is. Final sections refer some experi- 
mental results and give indications about future work. 



2 World Representation 

An effective environment representation for an autonomous mobile robot 
must describe all the essential information necessary for self-localisation, mo- 
tion planning and navigation. All those information should be easily extracted 
from sensory data. 

The mapping approaches, i.e. the way the world is represented, that have 
been proposed in literature can be grouped in two main classes [3]: metric 
maps and topological maps (see Figure 2.1). A metric map represents the 
environment in terms of geometric relations between the objects and a fixed 
reference frame. On the other hand, in a topological map, only adjacency 
relations between objects are represented, avoiding metric information as far 
as possible^. 

Metric maps and topological maps are two different representations of the 
same environment: as a consequence, they exhibit complementary rather than 
opposite properties. Metric maps are adequate to represent sensory metric 
information and are necessary for the implementation of metric based algo- 
rithms like the shortest path search. On the other hand, this representation 
is often space consuming, heavy for long range planning, sensitive to odomet- 
ric errors, and last, has a poor interface to symbolic system as, e.g. expert 
systems. 



^ The term “topological” in the title is indeed used in a loose sense to mean 
“without a metric” . 
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Figure 2.1. Metric (left) vs. topological (right) maps 



A topological map is typically a more abstract representation than a met- 
ric one and can be synthetically represented by a graph [7], even if the se- 
mantic associated to nodes and arcs can differ depending on authors’ defini- 
tions [14], [25], [10]. This is a compact representation that does not require 
accurate determination of the robot position for a (topologically) efficient 
planning. For this reason, and considering that the graph can be enriched 
with extra-topological information, it can be effectively used for symbolic 
planning and navigation in an unstructured environment and in particular 
when considering long displacements. Moreover, they lead to friendly man- 
machine interfaces as tasks are described (almost) without using metric infor- 
mation by phrases as “follow the corridor and turn right at the first corner” . 
The main disadvantage of topological maps is that direct extraction of ab- 
stract information from sensory data is quite a difficult process and can lead 
to a perceptual aliasing. Finally, precise motion planning is not feasible and, 
more often, only a suboptimal path can be computed to reach a goal. 

To overcome the problem concerning the difficulty of extracting abstract 
information from sensors, we can exploit the structure of the workspace, 
especially when the robot navigates in ofRce-like environments: this kind of 
indoor environment is usually structured with standard elements (places) like 
corridors, corners and doors. 

A graph can easily describe such environments, with nodes representing 
basic features and arcs symbolising the connections between them. This rep- 
resentation is well suited for long-range navigation: for example, consistent 
motion strategies can be attached to each node. One example could be a cor- 
ridor node with a set of strategies that can be explicitly suited for it (follow 
fast/slowly, follow near the right/left wall, etc.) and can be used to build 
a navigation sequence. Adding some more attributes can enrich the nodes. 
One attribute can be an approximate measure, for example the length of the 
corridor; other attributes can be the number of doors on the left and on the 
right. Clearly, in order to correctly execute the motion plan, the robot must 
localise itself on the graph, that is, the robot has to recognise in which node 
it is. 
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Therefore, a topological map is defined as a graph (see right side of Fig- 
ure 2.2), in which nodes correspond to places and arcs connect two nodes 
if they are adjacent in the real environment. In particular, we consider here 
only five kinds on nodes: four corresponding to the places corridor, corner, 
end-corridor and T-junction, plus an additional one named open-space, that 
can be useful when the corridor becomes too wide. Each node is augmented 
with (at least) the motion strategy, so that it can correctly represent together 
the environment structures and the relevant control strategies. 





Figure 2.2. Typical office-like environment and its topological representation 



3 The Overall Structure 

To describe the overall structure adopted in the research, we will follow the 
data flux, starting from the measures and ending with the decision about the 
node occupied by the robot. 

The main idea consists in comparing an egocentric representation of a 
“small” area around the robot with some prototypes representing the basic 



Landmark Recognition in Indoor Navigation by Fuzzy Maps and CBR 231 



features considered as the basis for the environment description. The compar- 
ison must be efficient and fast and thus noise and unnecessary details should 
be avoided. 

The first step consists in collecting the readings from the ultrasonic sensors 
as the robot moves, and to build a small fuzzy local map (FLM, as defined 
in next section) using data from the recent measures. This map generally is 
quite noisy, as the robot is moving and there is little time to collect many 
readings. Therefore, a hrst level of filtering may be useful to improve its 
quality: algorithms from fuzzy morphology have been found quite effective 
(see Section 5). The decision about the opportunity of this enhancement 
depends on the quality of the measures (and therefore on the kind of the 
environment) and on the available computing power, as it is rather time 
consuming. 

An efficient comparison requires reducing the information from two di- 
mensions to one dimension. We will see that, during this phase, the metric 
information are discarded and only the structural one is kept. The output of 
this step will be called a “worldmark”. Again, the data from this step can 
be improved, in particular if the two-dimensional filtering is skipped. To this 
aim, an approach based on wavelets theory can be considered that gives very 
good results. Moreover, it can be used to derive numerical coefficients for the 
comparison phase. 

The comparison between the worldmark and the available prototypes can 
be effected in several ways. In a previous work [9], the authors proposed a 
fuzzy similarity measure. It works fine, but has a small, static database of 
prototypes. A more flexible approach encompasses Case-Based Reasoning, 
and allows adding new prototypes dynamically to the database by a super- 
vised learning approach. Note that for each feature, there can be more than 
one prototype according, e.g. to the robot position during the measures. More 
comparison choices will be discussed, as a suggestion for further research. All 
the comparison techniques provide one or more matching prototypes sorted 
by their similarity with the worldmark. Often this is sufficient to decide the 
actual robot position, but adding information on the previous positions in- 
creases, obviously, the robustness of the decision. 



4 Fuzzy Maps 

A Fuzzy Local Map (FLM) can be dehned as a Fuzzy Map [18] representing 
the surroundings of the robot. This geometric map is divided into cells, and 
for each one, two values specifying the degree of membership to the set of 
empty cells and to the set of occupied cells are given. A FLM is thereafter 
represented by two fuzzy sets: the empty cells set £, and the occupied cells 
set O. The FLM is derived at each step merging the last n sets of collected 
data. It represents the world around the robot. 
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Those fuzzy sets can be employed in several navigation tasks, alone or 
combined to obtain new fuzzy sets like the one describing the degree of colli- 
sion danger of navigating into a cell. This set is often referred as navigation 
map {M) and can be used, for example, to plan fine motions to cross a door, 
or to go round an obstacle, or to escape from a blind alley. For its descrip- 
tion see [18]. Some M sets obtained from FLMs are reported at left side of 
Figures 4.1 and 4.2. In this case a FLM of 40 by 40 cells is used, each one 
covering a space of 10 cm by 10 cm. Note the difference between the white 
portion of the image corresponding to unknown cells and the light gray (near 
the robot) one related to very low danger cells. 




Figure 4.1. Map and World Mark for a corridor 





deg 



Figure 4.2. Map and World Mark for a corner 
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The possibility of representing the surrounding of the robot in a metric 
way using fuzzy logic gives us the ability of fusing information coming from ul- 
trasonic sensors as well as other kinds of exteroceptive sensors (laser scanner, 
etc.), and opens the way to the use of concepts coming from mathematical 
morphology. 



5 Fuzzy Morphology 

Mathematical morphology, based on digital topology theory [19] with its 
extension to grey-scale images [21] and fuzzy sets [23], is a typical image 
processing methodology and can be used to extract simple shapes from a 
digital image and to modify the image itself. 

The main idea is to examine the structural content of an image by match- 
ing it with a small pattern (“structuring element”) at various locations. Only 
two elementary operations, called dilation and erosion, are required to build 
up complex operators like closure and opening ones. If the structuring ele- 
ment is a white disk (left of Figure 5.1) the opening erases the white holes 
smaller that the disk. If the structuring element has a conical distribution of 
grey levels, that can be interpreted as a fuzzy membership function represent- 
ing “large open space” (right of Figure 5.1), the result after transformation 
is that the value of each cell will represent how much the cell belongs to the 
set of cells that are in a “large open space” . This means that very small holes 
will be definitively closed, but larger ones will be only decreased depending 
on their size. 

Those concepts can be usefully employed in a fuzzy navigation map: for 
example the shape of poorly mapped walls can be effectively bettered, as 
shown by Figure 5.2. Here, the application of the opening operator is pre- 
sented for the image of a corridor with many small holes. They could easily 
lead to misinterpretation during the comparison phase. The same operator in 
Figure 5.3 is applied to a T-junction image: the improved one, on the right, 
has less noise and can be better interpreted as a place with open space back, 
forth and on the right. The size of the structuring element is critical; indeed 
it is possible to “close” small holes but also doors along a corridor. 



6 Worldmark 

To ease the applicability of this approach to a real-time navigation problem, 
an important simplification is applied by replacing the full 2-D fuzzy descrip- 
tion with a polar representation. The J\f set (navigation map) is used to build 
a suitable 360 degrees horizon representation of the world around the robot 
{worldmark). The worldmark can be seen again a fuzzy set, calculated by 
associating to each direction a degree of emptiness of the space around the 
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robot. In particular, first the center of gravity of A/” is calculated, then 60 rays 
are taken from there, one every 6 degrees, along which the distance of the 
maximum value of collision danger (darkness) is taken as a measure of the 
(inverse of) degree of emptiness along that direction. The center of gravity 
is used to reduce the sensitivity to the actual robot position and to improve 
the representation of the spatial distribution of walls and empty spaces in 
the image. 

As the distances from the center of gravity are not used in the worldmark 
computation, this actually carries only structural information about the space 
around the robot. 

Two examples of worldmarks are reported at the right side of Figures 4.1 
and 4.2: note that “full” labels associated to each direction mean that there 
is a wall, while an “empty” label refers to the presence of a free space (in 
the short distance). As it can be seen in Figure 4.2 the angular shape of the 
corner is not caught in this simplified analysis while it is correctly understood 
that there are two free spaces in orthogonal directions. This can be enough 
for the aim of navigating in ofBce-like environments but pushes for a more 
accurate investigation to extend the accuracy of this technique. 

Now, the general comparison problem is reduced to matching this world- 
mark with p prototypes from a static or dynamic library. This matter will be 
developed in Section 8. 



7 Wavelet Decomposition 

Wavelet Transform theory is a rather recent concept, since the hrst references 
in the literature date back about 10 years [15], [11]. Their success is linked to 
their specihc property whereby they may be located both in time (space) , as 
well as in scale (frequency), thus providing a “time-scale map” of the signal, 
wherefrom it is possible to extract time varying features. Due to this special 
property wavelets are an ideal tool for the analysis of physical situations char- 
acterised by signals of a discontinuous nature and featuring sharp variations, 
as is our case. The analysis procedure, by means of wavelets, is based on the 
use of a prototype function, so-called “mother wavelet” , whose translated and 
extended (or compressed) versions constitute the basis functions for the series 
expansion that allows for the representation of the original signal through 
coefficients. Data operations may therefore be effected on the correspond- 
ing wavelet coefficients. In particular, if the mother wavelet is appropriately 
chosen, or if the coefficients under a certain threshold are eliminated, it is 
possible to “sparsely” represent the original data. This means that wavelets 
are a most useful tool in the context of data compression. The practical, fast 
computation of wavelet transform has become possible in applications, owing 
to the fast DWT, also known as Pyramid Algorithm [15]. This algorithm is 
0(n), where n is the length of the array of data to be transformed; this is 
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extremely advantageous compared to other algorithms (for example the FFT 
is O(nlogn)). 

7.1 Denoising with Wavelet 

The above-mentioned considerations explain why wavelets found so many 
applications, in particular when dealing with noisy signals. To resolve very 
noisy problems different techniques have been proposed [5] , [6] , based on the 
following idea: the decomposition of a data set by means of wavelets involves 
filters that output both “averages” and “details” ; if the latter appear of minor 
significance, they can be attenuated or eliminated (“wavelet shrinkage or 
thresholding” ) without causing a substantial alteration of the main features 
of our given data set. This method is exemplified in the diagram shown in 
Figure 7.1. 



RAW 



PROCESSED 




Figure 7.1. Data Analysis by Wavelets 



Note that the signal is initially transformed, its coefficients are matched 
against a predefined threshold, and subsequently reutilised to reconstruct the 
original data set through the inverse DWT. Donoho e Johnstone [5], [6] pro- 
pose a number of different denoising strategies, using predefined thresholds 
as well as adaptive ones. Actually, there is no one strategy that always gives 
the best results compared to the others, but only strategies whose validity 
depends on the type of application. Once the threshold is determined, it may 
then be matched against the wavelet coefficients using two different possible 
methods (see also [16], [17]): 

— Hard Thresholding , which implies the “keep or kill” principle, i.e. all the 
wavelet coefficients are checked at an absolute value against a predeter- 
mined threshold th'- if the magnitude of the coefficient is less than th, the 
coefficient is replaced by zero, otherwise it is left unchanged: 

/ O5 ^jk ^ Ih 

\ ^jk A th- 

— Soft Thresholding , in this case, the wavelet coefficients are shrinked towards 
the origin in accordance to the following expression: 



Cjk — sign(^Cj]i'){^\cj]i\ 
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Figure 7.2 reports the result of the application of the two different meth- 
ods to the signal shown at the top, using the same threshold value [th = tg 
= 1.1) in both cases (the 3-scales forward and inverse DWT are performed 
by using Symmlet with 8 vanishing moments as the mother wavelet). 



8 Similarity Metric 



In this section some techniques that have been extensively tested on the 
available data will be analysed. The problem is to dehne a similarity function 
between the current worldmark and the reference ones. Till now we have de- 
fined five kind of places, drawn in Figure 8.1 together with their worldmarks. 

Such distance function must satisfy the following four basic properties: 



A{x,x) = 0 
A{x,y) > 0, x^y 

A{x,y) = A{y,x) 

A{x,y) < A{x,z) + A{z,y). 



( 8 . 1 ) 



In a real-time context, a good metric must guarantee an efficient compromise 
between two main requirements: quality and computational complexity. To 
this aim, it will be shown how the effectiveness of the metric will depend 
on the preprocessing phase, i.e. on the ability of removing extra information 
leaving only the part functional to the recognition purpose. 



8.1 Cross Correlation Similarity 

In the context of the experimentation, we adopted different approaches, each 
of which yielded its specific merits and shortfalls. Among these, the best 
overall results were given by the Cross-Correlation Factor metric, expressed 
as follows: 

max(£xy(r)) 

a/ (£xx(0)£yy(0)) 

This quantity was calculated both in the time domain and in the Fourier 
domain, obtaining important results, with reasonable computational lengths 
using calculation resources available on the market. Furthermore, this method 
allowed us to compute the other information available in the mono-dimensional 
map, that is, the polar information. 



8.2 Fuzzy Similarity 

Another approach followed, based on the use of fuzzy sets theory [12], con- 
sists in computing the degree of matching between the measured worldmarks 
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and the reference ones, interpreted as fuzzy sets. The comparison was per- 
formed by using one out of the many fuzzy similarity functions defined in 
literature. In our case the following fuzzy similarity function defined by Sokal 
and Michener, reported in [2], gave reliable results: 



sim(AI, £) 



card(AI n £) -I- card(AI n C) 
card(AI n £) -I- card(AI n £) -I- card(ZY) 



(8.3) 



where M is the fuzzy set representing the world mark, £ is a reference set, 
and U is the universe of discourse. In particular, as intersection operator, the 
minimum function has been used and the cardinality is defined as the integral 
of the membership function. 





Figure 8.1. Reference sets 



To take into account that the relative orientation between the robot and 
the real feature is not a priori known several comparisons have been per- 
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formed shifting A4 w.r.t. £ to cover the possible orientation range. The final 
resemblance Res(£) to a feature is computed as the maximum value of all 
similarities obtained during this process. This step is concluded with the 
delivery of the p resemblance measures that are in the interval [0, 1]. 



8.3 Wavelet Similarity 

Using the concepts of thresholding and of DWT, it is possible to define a new 
method to compute a similarity function based of wavelet decomposition. 

First, a complexity reduction of the original signal (the worldmark) is 
performed extracting only few significant coefficients from the DWT, and 
second, comparing those coefficients with the ones stored for the ideal features 
in the library. In particular, the forward 3-scale DWT (with Symmlet and 8 
vanishing moments) is computed and of the 360 obtained coefficients, only 45, 
coming from the low-pass filter of the third scale, are retained. Comparisons, 
using those coefficients, can be made in several ways: the one here adopted 
is again a normalised cross-correlation. Experimental results, comparing the 
time required for this cross-correlation (including that to compute the DWT) 
with the one made on the whole worldmark, give a reduction of more then 
1/4 with no difference for what concerning the feature recognised. 

Possible improvements are in the denoising phase that envisages the appli- 
cation of the forward and inverse DWT. Therefore, an interesting objective 
is to optimise the identification of the mother wavelet and the number of 
scales to which the wavelet transform is applied. For now, this choice is pre- 
determined, but it should be possible to develop a model where this happens 
dynamically, on the basis of feedback on the obtained results. 

In the next section we will see how the possibility of reducing the amount 
of data stored for each worldmark open the way to the use of a larger library, 
possibly to build starting from data collected during navigation. 



9 Extension to a Dynamic Library: CBR 

As stated before, the first recognition phase is done in most cases comparing 
the actual view with a static list of models obtained with a priori consider- 
ations on the environment itself [9]. Instead, following a CBR philosophy, a 
learning approach can be devised in which several real-world cases are ob- 
tained from a supervised navigation and used to build a dynamic library [13], 
[1]. In this section we want to show how such a method can be successfully 
applied to this problem in dynamic environments containing features that 
only partially correspond to the already-known cases. 

Assume to have a new worldmark, see Figure 9.1, to be classified. The 
retrieval module shown in the figure will search in the case library containing 
the old cases, with a <problem representation, solution> structure, which in 
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CATEGORIZATION MODULE 




Figure 9.1. Case-Based Approach to Image Recognition for Indoor Navigation 



this specific case will be <worldmark, topological feature represented by the 
image>. The solution given in the old case can therefore be seen as a pointer 
to a new library, which can be named “Library of Objects” , containing the 
categories (i.e. “topological features”) that could be present in the worldmark 
to be analysed. The “Recognised Feature” is at this point taken into consid- 
eration by the robot navigation system to plan its motion. This object, which 
constitutes the old solution of the case retrieved from the Case Library, will 
also be considered as the solution of the new problem (basically, there is no 
need for an adaptation of the old solution to suit the new case) and if the 
supervisor accepts it, the pair <New Worldmark, Recognised Feature> can 
be inserted as a new case in the Case Library. 



FUZZY 

MAP 



>- 



IMAGE 

RECOGNITION 

MODULE 



IDENTIEIED 

IMAGE 

► 



Figure 9.2. Image Recognition Module 



Let us now analyse further in detail the architecture of the module shown 
in the Figure 9.2. For sake of clarity, we provide a simplified version to il- 
lustrate the problems addressed and the solutions proposed. The algorithm 
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has been implemented in C since the entire control procedure in respect to 
the robot’s navigation was originally developed using this language. For a 
better understanding of the procedure, the algorithm reported in Figure 9.3 
is based on the use of simplified data structures as opposed to the ones actu- 
ally implemented. We assume the use of a record consisting of two fields for 
the case representation, both as concerns the new case as well as any other 
in the Case Library. The first field is reserved to the representation of the 
image. Therefore, the “new digital image” practically consists of an array of 
360 real numbers between 0 and 1 (the worldmark) . Instead, the second field 
is reserved to the recognised object, where “object” is intended as an integer 
corresponding to one of the above possible categories. Accordingly, the set of 
instructions given in pseudo-code is as described in Figure 9.3. 

This structure envisages the possibility of intervention by a human expert, 
who is charged with the initial training phase of the robot as well as the 
checking phase of the retrieved solution. As shown, the use of two different 
threshold values Sa and Sb are postulated and the former controls whether 
the case solution retrieved from the library is, or is not, extended to the new 
case. Basically, Sa is the “reliability threshold” that rates the possibility for 
the case extracted from the library to be considered as representative of the 
one under observation. Instead, the possibility of the insertion of the new case 
in the Case Library is dependent on Sb, the so-called “identity threshold”. 
The reason for the existence of this second threshold is obvious. Indeed, a 
richer case library increases the possibility of retrieving the best matching 
case to the input one and, consequently, improves the accuracy of matching 
the found solution to the new situation. 

On the other hand, an extensive library involves two drawbacks: 

— more time is required to retrieve cases from memory; 

— more memory capacity is needed. 

In fact, if we imagine the Case Library as an array of records, each time a 
case is input to our module, the procedure is run through from the beginning 
to the end, therefore the computational complexity of this operation is linear 
to the number of available cases. As a matter of fact, these are actually 
underlying problems common to all CBR applications, as also demonstrated 
by the considerable size of material generated by the Artificial Intelligence 
community on these concerns. Various solutions [20] have been proposed, 
which can be grouped into three categories: 

— flat memory; 

— discriminant nets; 

— E-MOP (Episodic Memory Organisation Packets). 

On the basis of the obtained results, we can affirm that the computa- 
tional burden ensuing from a denoising operation appears compensated by 
the signihcant reduction of cases to be inserted in the library. This is due to 
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Function REC(NewImage) returns RecDbject 

inputs : Newimage ; the input image 
variables : CaseLib; the case library 
Cj ; the generic old case 
Sa ; the reliability threshold 
Sb', the identity threshold 
local variables : D. image; the image representation 
D. object; the recognised object 
Sj ; the metric value 

tempvalue ; the temporary metric value 
tempind; the temporary case index 

D. image ^ DENOISE (Newimage) 

D . object ^ 0 
tempvalue ^ 0 
tempind ^ 0 

for each old case Cj in CaseLib do 
begin 

Sj ^ COMP ARE_CASE(D . image , Q . image ) 
if (tempvalue < Sj) then 
begin 

tempvalue ^ Sj 
tempind ^ j 
end 

end 

if (tempvalue < Sa) then 
begin 

D. object ^ HumanExpertSolution 
C„+i ■ image ^ D . image 
C„+i. object ^D. object 
end 
else 
begin 

if (Ctempind • object = HumanExpertSolut ion) then 
D . ob j ect ^ Ctempind • ob j 6Ct 
else 

D. object ^ HumanExpertSolution 
if (tempvalue < Sb) then 
begin 

C„+i . image ^ D . image 
C„+i. object ^D. object 
end 

end 

RecObject ^D. object 
returns RecObject 

Figure 9.3. The Image Recognition Algorithm 
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the fact that by eliminating the overlaying noise the similarity of two situa- 
tions corresponding to the same topological condition may often be revealed, 
while their respective representations covered by noise misleadingly appear 
considerably different. Moreover, we have already seen how the computation 
length and the quantity of utilised resources are directly proportional to the 
number of cases kept in memory. 



10 Combined Recognition 

The first step of the recognition problem ends with the formulation of the 
“best” feature matching the actual sonar view. This cannot satisfy all the 
needs for a correct navigation which, in principle, should be prepared at 
different source of noise: at least people moving around the robot and giving 
it a wrong “impression” of the environment. This advocate for a method 
able to fuse data and information of different nature and coming from past 
navigation history such as external agents. 

Thereafter, for the second step of the recognition process, an algorithm 
can be devised on the base of the Transferable Belief Model theory (TBM) [4], 
[22] , [24] . This method is based on the use of the similarity function adopted 
in the first recognition phase as an agent that assigns a degree of belief on 
each feature when new data become available. Those beliefs, to add robust- 
ness with respect to disturbances coming from multiple reflections or unex- 
pected obstacles (e.g, furniture, walking people, etc.), are combined with past 
information or any other belief coming from other independent agents. For 
example, if the node was labeled with a length I, this information could be 
used to force an opinion that the corridor is reaching its end as far as the 
robot is estimating that the path traveled is approaching 1. 

Let X = {C, R,T, E,0} be the universal set containing the corridor C, 
the corner R, the T-junction T, the end-corridor E, and the open-space O. 
Let X be an element of X , and the event E an element of P{X), the power 
set of X. All the possible events 17 include the empty set (contradiction) and 
the universal set (unknown). 

At each step, a basic mass assignment (BMA) mr{F) is determined: this 
function quantifies how much the event F is supported by the available in- 
formation. In our case, considering the fuzzy approach to the evaluation of 
similarities, the available information consists of the resemblance values cal- 
culated in the static recognition. A possible BMA can be obtained supposing 
that each resemblance Res(£) supports with its value the atomic event L as- 
sociated to the reference set £, and with its negate both the complementary 
and the unknown events: 

{ Res(T)/p VA atomic with F = L 

(1 — Res(T))/2p ifF = L (10-1) 

(El (1 - Res(T))/2p ifF = X 
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This mass distribution guarantees that BMAs add to one. 

To update the past BMA mk-i{F) and combine it with the new BMA 
mr{F) the following rule of combination is used: 



mk{F) = ^ mr{G) mk-i{H). ( 10 . 2 ) 

GnH=F 



Now, following [24], the probability function BetP, used to make decisions, 
can be written as: 



BetPk(F) = 



mfe(G) 



(10.3) 



Note that BetP(-) is computed only for the atomic events F: comparing those 
values the best hypothesis and its level of belief can be determined. 
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Figure 10.1. Resemblance and Bet for the corridor 



To give some examples, consider Figures 10.1 and 10.2: they show the 
resemblance and bet values relative to Figures 4.1 and 4.2 respectively. Note 
the difference between resemblance, that are related to one reading only, 
and bet values, taking into account the history due to past readings: e.g. in 
Figure 10.1, the hnal degree of belief in the corner hypothesis is lower than the 
corresponding resemblance value due to the conflicts between this hypothesis 
and both other ones and past history. Moreover, in case of erroneous readings, 
the method is able to filter out incorrect resemblance. 

An important role plays the mass of the empty set. Remember that this 
value increases every time some contradiction is produced by resemblance 
functions. So, as far as the robot navigates and changes its world mark, its 
destiny is to collect all the masses distributed among the possible events and 
to reach the unitary value. This would imply a freezing of the bet values to a 
zero probability. Now, to allow this changing of opinion, at every iteration a 
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Figure 10.2. Resemblance and Bet for the corner 



small percent of the empty set mass (10%) is reversed into the universal set 
and can be used, through the TBM, to feed new opinions. Finally, the mass 
of the empty set can be also used to judge the quality of the bet values: when 
increasing, the level of contradiction of measures is high and an extra analysis 
could be performed, for example slowing the robot and allowing more sonar 
readings. 

Note that the choice on how distribute the BMA could be changed if, for 
example, we would like to reward the winner (the one with the maximum sim- 
ilarity) and all the subsets containing it and the other atomic elements. As op- 
posed to the previous choice, we never attribute a mass to the atomic elements 
that are not winning the “competition” . Looking at Figure 10.3 we see how 
masses are distributed only to the sets {C}, {C, E}, {C, T, E}, {C, T, A, O}, 
and {C,R,T,E,0} (unknown). In this case, results obtained show the sys- 
tem less incline to change its opinion when a new feature is approaching due 
to a stronger bet value, but more hrm in devising the right one in presence 
of some noise. 

From experimental tests, we found that the proposed recognition algo- 
rithm is computationally efficient and, thus, can be performed in real time 
during navigation. The complete cycle, including acquisition, FLM building, 
feature recognition, and motion control with an obstacle avoidance algorithm, 
takes about 0.5 s on a Pentium I at 133 MHz running Linux. 



11 Experiments with Static Library and TBM 

The method has been tested on a Nomad 200 equipped with a ring of 16 
equally spaced ultrasonic sensors. The environment used for the experiments 
is a branch of our department, schematically depicted at left-side of Fig- 
ure 2.2. On the right side of the same hgure is shown the corresponding 




Landmark Recognition in Indoor Navigation by Fuzzy Maps and CBR 



247 




C R T E 0 O 



Figure 10.3. Alternative Basic Mass Assignment 



graph that can be easily built by the user. The oriented arcs in Figure 2.2 
represent a predefined navigation plan: the label on the node inside the round 
brackets specifies the motion that the robot has to execute when it is in that 
node. 

To demonstrate the validity of the proposed approach, let us consider a 
sequence of three steps near the goal, shown in Figure 11.1. On the left side, 
we can see the local fuzzy maps built from ultrasonic measures collected in the 
last three steps. This is done to have more information about the surrounding 
space and to filter out spurious reflections. On the right, the beliefs on each 
feature are represented. 

Robot in the corridor: the robot sees two large empty spaces in the front and 
in the rear. Resemblances give for many steps a large contribution to the 
mass of the corridor and the belief on this feature is much higher than 
on the other ones. The robot is navigating into the topological map and 
is performing a follow corridor behaviour. 

Robot between the corridor and the corner: now beliefs are almost the same 
because of the ambiguity of the sensory data. The empty set mass (not 
reported) is increasing so the robot should be slowed down. Actnally, this 
is automatically done by a lower level safety module that is not described 
in this chapter, an obstacle avoidance module [8] that forces the robot to 
move only inside a safety area and decreases robot speed when obstacles 
are too near. 

Robot in the corner: the new feature has been correctly and repeatedly iden- 
tified so its bet is now sensibly higher than others. Robot position inside 
the topological map is updated and a new behaviour will be executed 
to turn the robot left and step into the next corridor. The latter will be 
recognised very soon. 
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Figure 11.1. Transition from a corridor to a corner 
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12 Conclusion and Future Developments 

This chapter has presented a structure for indoor landmark recognition. It 
is composed by several layers. Using this structure, several techniques that 
can be usefully adopted have been described, in particular as it concerns the 
“highest” layers. 

An interesting proposal is to use CBR to compare the acquired data with 
a dynamic library of prototypes. 

Indeed, the normal pattern recognition techniques require models of the 
objects that must be recognised and classified. The collection of models avail- 
able to the classifier clearly reflects the original comprehension of the situation 
to be analysed. Sometimes an a priori knowledge can be formalised and used 
to build such models; however, in most cases, as for the robot’s autonomous 
navigation, there exists practically no prior information whatsoever. If a tra- 
ditional feature extraction algorithms is incorporated into a CBR system, a 
constant increase in the knowledge of the surrounding environment is possi- 
ble. Furthermore, it should not be forgotten that there is also the possibility 
of updating the Object Library as well as the Case Library. 

Finally, a second step can be taken to improve the navigation algorithm. 
To fuse past and actual believes and merge approximate metric information 
with a similarity metric, in view of a correct deduction of a final belief about 
which feature is more plausible, a Dempster-Shafer approach can be success- 
fully applied. 
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In this chapter the localisation problem for mobile robots is addressed. The 
localisation is accomplished using data coming from different sensors, which 
must be properly combined. For this reason, as a preliminary material, the 
first two sections of the chapter will be devoted to sensor integration and fu- 
sion. Different approaches will be considered according to the specific scenario 
where the localisation must be executed. The localisation section will present 
some basic methods and problems and will also report some experimental 
results. The chapter ends with a conclusions section which summarises the 
subject and also provides some directions for future research. 



1 Introduction 

Let us consider the problem of implementing a control system that enables 
a robot to autonomously execute a given task in a given environment. Let 
us call world the set of robot and environment. The robot control system 
receives information about the world through robot sensors and modifies the 
world through robot actuators. This is illustrated in Figure 1.1. 

An architecture provides a set of principles to organise a control system. 
There are four basic families of control architectures: 

1. planner-based, 

2. reactive, 

3. hybrid, 

4. behaviour-based. 

Traditional Artificial Intelligence (AI) systems are planner-based systems. 
The system is decomposed in a set of functional modules, as domain in- 
dependent as possible to increase module reuse. The module interaction is 
mediated by an internal central world model. The control structure is sequen- 
tial: each module takes its turn to interact with the internal representation 
of the world. In general, sensory information is fused by a perception module 
to update the internal model. Then a reasoning engine, the planner, works 
on the internal model to generate a sequence of actions. The internal model 
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should be as complex as the external world, and this fact makes planner- 
based systems not workable for complex environments, in particular in the 
dynamic case. 

A reactive system consists of a set of condition-action pairs (if necessary, 
there can be a short-term memory): this system reacts to each “stimulus” 
with a pre-programmed action. It is important to have a fast feedback from 
the environment to ensure that the appropriate action is executed. A set 
of very simple reactions can determine a complex behaviour. For complex 
environments, it may be very difficult to associate an action to each input. 




Figure 1.1. Multisensor integration 



Hybrid systems combine the potentiality of reactive and planner-based 
systems. Usually a reactive system handles some low-level real-time tasks (for 
example to avoid dynamic obstacles) while a planner-based system handles 
higher-level tasks. As the reactive module can change the world model or the 
robot state, the planner must be able to re-plan consequently. 

Behaviour-based systems consist of task-oriented modules, working in par- 
allel and independently producing commands in response to their particular 
view of the world (computation and representation are distributed contrary 
to planner-based systems). An arbitration method is necessary to select or 
fuse the different commands. The modules communicate directly with very 
short messages or indirectly through the environment (that is used as a sort of 
“external model”). A coherent behaviour emerges from modules interaction 
with the environment. In Figure 1.2 a comparison between a behaviour-based 
architecture and a planner-based one is showed. Hierarchical structures like 
those in Figure 1.2 have been also employed for structured environment rep- 
resentation maps in [6]. 

The attention will be focused on the first kind of architecture and in 
particular on sensor interpretation problem. The main objective is the de- 
termination of the robot position in the environment {localisation problem): 
in order to do it, sensory data must be processed according to some in- 
terpretation rule. The precision and accuracy in the position estimation, the 
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Figure 1.2. Examples of behaviour-based and planner-based architectures 



robustness of the localisation method and the reliability of the system are ba- 
sic requirements in robot localisation. The use of a unique sensor can hardly 
satisfy these requirements. First, a high precision sensor is much more expen- 
sive than many low precision sensors. In addition, the use of a unique sensor 
can seriously impair the robustness and the reliability of the system. Require- 
ments like precision, robustness and reliability can be greatly improved using 
a multi-sensor system. 

However, data coming from different sensors must be properly integrated. 
The concept of sensor integration generically denotes the combined use of 
different types of sensors. Sensor fusion is a more specific concept, which 
indicates that data from different sensors are combined into one representa- 
tional format. Redundancy, complementarity, timeliness, uncertainty reduc- 
tion, reliability are the advantages offered by the use of sensor fusion. Sensor 
integration will be the subject of Section 2, where the concept of sensor close- 
ness (that is data consistency among different sensors) will be also presented. 
Section 3 presents the specific topic of sensor fusion. 

A different approach is required if the environment statistics are known 
or unknown. This will be discussed below together with centralised and de- 
centralised architecture implementation. 

A typical application of sensor fusion is the mobile robot localisation, 
which will be presented in Section 4 and is the main subject of this chapter. 
A Kalman Filter can be used to combine data from the sensors and to localise 
the robot if an environment representation (let us call it map) and a sensor- 
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environment interaction model are available. If the map is not available, the 
localisation process and the map construction can be carried out jointly as will 
be discussed below. More complex localisation problems, related in particular 
to space exploration missions, concern the localisation of a team of mobile 
robots. Experimental results and concluding remarks giving future research 
directions are also included in this chapter. 



2 Sensor Integration 

2.1 Techniques and Approaches 

Multi-sensor integration and fusion have been used in the last years in many 
areas. Typical applications are industrial tasks like assembly, multi-target 
tracking and aircraft navigation. The use of sensors with different character- 
istics is also common in mobile robotics, and it is well known that integration 
and fusion techniques allow to largely improve the performance of the overall 
system. The problem in this research area is to find strategies to integrate 
the information coming from different sensors in order to assist a system in 
the accomplishment of a task. This topic can be achieved either by using the 
information from one sensor to provide cues or guide the operations of the 
other sensors, or by fusing the information from multiple sensors. 

Integration and fusion approaches have usually different aims. In partic- 
ular, the sensor integration concept is mainly used to generically indicate 
the combined use of information from sensors of different types, no needing 
a common data representation for different sensors. Sensor fusion is carried 
out at a lower level, with the specific purpose of combining information from 
different types of sensors, usually with different data structure, into a single 
data representation, and with a degree of accuracy higher than single sources 
one. In particular, sensor fusion can be carried out both at the level of consec- 
utive sets of data from the same sensor, and at the level of data from different 
sensors (see, among many others, [35], [1] and the references therein). 

In other words the multi-sensor integration consists of the synergistic use 
of the information provided by multiple sensory devices in order to achieve a 
particular task; the multi-sensor fusion consists of one stage in the integration 
process where data from different sensors are combined into one representa- 
tional format. 

Clearly the overall architecture of the system strongly depends on the spe- 
cific sensor and on the particular form of the information it provides; moreover 
it depends on the particular task for the entire system. Because of this vari- 
ety of factors it is very hard to provide general methods able to enclose very 
different aspects of multi-sensor integration. Therefore, the results available 
in literature consist of a great amount of frameworks, paradigms sometimes 
experimentally tested. Many of these paradigms come from computer sci- 
ence, control theory, systems analysis and artificial intelligence. Among these 
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frameworks, the notions of modularity^ hierarchical structures and adaptabil- 
ity have a particular importance. 

The modularity of the integration process design reduces system complex- 
ity and increases its flexibility and possibility of being modified in relation to 
sensors kind and features. This is typical of structured programming. 

Usually, the structure of the information processing system is intrinsically 
hierarchical. Data from the sensors are transformed into representation pro- 
gressively more abstract, moving from the low level of the sensors up to the 
higher level, where information may become symbolic to represent complex 
concepts such as moving obstacles, target positions, landmarks and so on. 
This corresponds also to different control and planning levels in a naviga- 
tion architecture. For example, the feedback motors control is usually based 
on current position measurements/estimates, while obstacle avoidance algo- 
rithms consider data at higher levels, and trajectory planning may take into 
account information at a further abstract level. 

Finally integration process adaptability can be an efficient mean to over- 
come multi-sensor integration errors and uncertainties. 

The basic advantages of sensor fusion and integration are: redundancy, 
complementarity, timeliness and cost. As a matter of fact, data from sev- 
eral sensors, whenever related to the same portion of the environment, are 
naturally redundant, and may allow uncertainty reduction. Similarly, if each 
type of sensor is specifically designed for a different feature, then the natural 
complementarity allows to reduce overall error. Finally, timeliness allows a 
faster data processing, while fusion and integration of low cost sensors may 
turn out in an overall sensing system with a good cost /performance ratio. 

The role of multiple sensors in the operation of a particular system can 
then be defined as the degree to which each of these four aspects is present 
in the information provided by the sensors. 

Data fusion has a great importance in mobile robotics. It is fundamental 
in order to integrate a localisation method that does not use beacons or 
markers, with a map-building algorithm that builds and constantly updates 
the world model (for more details see [2], [25], [17] and references therein). 

It is important to consider the problems related to a given multisensor 
integration architecture. Because of the absence of a general methodology for 
multisensor integration and fusion (it in fact strongly depends both on the 
particular task to be achieved and on the sensors used), any integration archi- 
tecture requires a careful planning phase. It is therefore necessary to analyse 
in detail the mobile robot (maximum speed, dimensions, vehicle model and so 
on) and the environment where it moves (indoor/outdoor characterisation), 
in order to find a good solution. Moreover, the architecture must manage 
data in a fast and efficient way because of the increased number of data, due 
to the increased number of sensors. Before sensor data can be fused, it is 
very important to make sure that these sensor data are consistent, in other 
words it is necessary to address the topic of sensor closeness [46]. Finally the 
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results obtained by many integration architectures are based on particular 
assumptions about how the sensors interact with the environment. In many 
contexts, as in outdoor environment, this interaction is hardly predictable. 

A quite general model for the multi-sensor integration process is shown 
in Figure 2.1, where a general case with n sensors is considered [35]. In the 
following. Si denotes the sensor. 




Figure 2.1. Multisensor integration 



Clearly any integration architecture needs to know the information ac- 
curacy of each sensor in order to weigh all the data in a proper manner. In 
other words, a sensor model (indicated in the figure with S.M.) is required. 
After data have been modelled they can be integrated. It is possible to dis- 
tinguish three different types of sensory processing [36]: fusion, guiding and 
other manipulation. 

In the figure the data corresponding to sensors from Sm to Sn are fused. 
In order to do it, the data must be firstly made commensurable. Sensor 
registration consists of any data operation finalised to test data closeness, i.e. 
to understand if data refer to the same location in the environment over the 
same time period or not. Let X and Y be two sensory data vectors generated 
by two sensors and Sx and Sy their covariance matrices. In order to test their 



Sensor Fusion for Robot Localisation 



257 



closeness a distance function must be introduced. Usually the Mahalanobis 
distance is used as matching criterion [53], [52], [46]. It is defined as follows: 

(f{X,Y) = [X-YY{Sx+Sy)-\X-Y). (2.1) 

In the case data provided by more than two sensors must be fused, it 
is necessary to have an algorithm able to determine which sensors refer to 
the same feature (reliable sensors) and which not (unreliable sensors). The 
Single Linkage Algorithm (SLA) is a useful way to achieve this result [46], 
by properly grouping sensors (sensor clustering). 

A guiding type of sensory processing refers to the situation where the data 
from one sensor is used to guide the operation of other sensors. A typical 
example of this type of multi-sensor integration is found in many robotics 
applications where visual information is used to guide the operation of a 
tactile array mounted on the end of a manipulator. 

2.2 Outdoor and Indoor Characterisation 

Most of the methods for autonomous mobile robot navigation have as a pre- 
liminary step the recognition of landmarks in the environment, whose position 
is known. The robot must be able to recognise the landmarks using its sen- 
sors. A good landmark should be easily identified. In order to simplify the 
problem of landmark identification one supposes that the robot position and 
orientation are known with some accuracy. In this way the robot can reduce 
significantly the exploration area. 

It is possible to distinguish two kinds of landmark: natural landmark and 
artificial landmark. The artificial landmarks are particular markers or ob- 
jects introduced in the environment with the unique purpose of allowing 
autonomous navigation. On the contrary, the natural landmarks are objects 
or features already present in the environment, independently of the robot 
navigation. Clearly natural landmarks are more effective in an indoor envi- 
ronment, since it is easier to recognise particular geometrical features. This 
is achieved by different methods depending on the sensor used (for example 
Hough transform is very popular to detect lines from video images). 

Concerning the navigation in an outdoor environment, the relocalisation 
method drastically changes. Most of the times means like the Global Posi- 
tioning Systems (GPS) or compass are used but in some applications it is 
not possible (for example in space applications). In this case it becomes very 
important to improve the odometric localisation accuracy [9] . 



3 Sensor Fusion 

In a mobile robotics context, sensor fusion consists in the direct integration 
of sensory data finalised to estimate robot parameters and/or state. The esti- 
mates obtained will be provided to motion planning and control algorithms. 
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In this section a classification of the different sensor fusion techniques to- 
gether with some illustrative examples is reported. 

The classification presented in this section follows the survey paper in [27] . 
It is based on the statistics knowledge about sensors and environment (known 
or unknown) and on fusion algorithm architecture (centralised or decen- 
tralised) . 

3.1 Sensor Fusion With Known Statistics in a Centralised 
Architecture 

When the sensor system and the mobile robot environment statistics are 
known, the sensor fusion problem is well posed and, if a centralised archi- 
tecture is used, the solution can be derived using well developed techniques 
based on the Kalman Filter (KF) or the Extended Kalman Filter (EKF). 
Game theory and Bayesian team theory are also used. Such techniques de- 
rive from the application of Maximum A Posteriori (MAP) and Maximum 
Likelihood (ML) estimation methods. 

Assume the robot is provided with a set of sensors. They can be seen 
as members of a team whose purpose is to determine a parameter a: € 3?". 
Sensors give noisy measures yt G 3?^, i = 1, . . . , A, related to the parameter 

X. 

Consider the case a; is a constant parameter: the case a; is a time varying 
quantity (such as the case of state estimation) will be considered in the 
localisation section. 

The recursive KF is first introduced following the approach in [4] in the 
case the measurement yi depends on the parameter x according to an 
equation of the following type: 



yi = CiX + u^, (3.1) 

where Ui is a noise component whose statistics satisfy: 



E[u^] = 0 

E[uiuJ] = Ui>0 

E[u^uJ] =0, j. 

These are reasonable assumptions, since it is always possible under proper 
transformations, to have a similar scenario [4]. The KF, if no Gaussian as- 
sumption is taken on the noise, provides the best (i.e. with minimum variance) 
linear unbiased estimate of x. This means that among all the estimators which 
seek Xk as a linear combination of the measurements {yi}, it minimises the 
expected error squared norm, i.e. 

E [(Ffe - x)'^{xk - a;)] 
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with E[xk] = X. Initially an estimate Xq of x is given together with its covari- 
ance matrix So- The estimation process evolves according to the following set 
of equations: 



Xi — Xi—i Kiigji CiXi—i) 


(3.2) 


K, = S.-iCf ([/, + CA-iCf)-^ 


(3.3) 


S, = (I -K,a)S,-i. 


(3.4) 



The term {yi — CiXi-i) is called the innovation: the new estimate Xi is ob- 
tained from the previous estimate by adding an amount proportional to the 
innovation. The proportionality factor Ki is the Kalman gain. The final es- 
timate Xn will minimise: 



N 

Q = {x- xo)'^Sq'^{x - xo) + - Cix)^U~^(yi - Qx), (3.5) 

and Sjv represents its covariance matrix, i.e. 

Sn = E [(xjv - x){xn - xf'] . 

Under a Gaussian assumption on the noise, also the estimate Xn will be 
Gaussian with xn ~ N{x, Sn). In this case the KF provides the best unbiased 
estimate among all (i.e. even non linear) estimators. 

Gonsider now the general case, where the dependence of the measurements 
on the parameter x to be estimated is not of the type (3.1). In this case it is 
possible to use the EKF. Let Zi G 3?™, z = 1, . . . , be the measurement of 
some unknown quantities Zi and assume that the relation between each one 
of these quantities and the parameter x is given by: 

/j(2:i,a;) =0, i=l,...,N, (3.6) 

where fi is a function from S?'" x 3?" into 3?^. Equation (3.1) is a particular 
case of this equation. The measurement Zt is affected by some additive noise 

Wi\ 

Zi=Zr+Wi, (3.7) 

where Wi is a random error with zero mean, known covariance and white 
noise: 

E[w^] = 0 
E[wiwJ\ = Wi > 0 
E[w^wJ] = 0, z j. 

It is possible to linearise the (3.6) if an estimate Xi-i of parameter x based 
on the first z — 1 measurements and an initial estimate Xq is already available. 
A first-order Taylor expansion of fi{zi, x) performed around {zi, Xi-i), which 
is a known point since Zi is the z*^ measurement, gives: 
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^ fi ^ Ji 

f^{Zi,x) =0'^ fi{Zi,Xi-i) + {x-Xi-i). 

(3.8) 

This equation can be written like equation (3.1) by setting: 



Vi — \(zi,Xi-i) Xi—i 

dji 



a = 



dx 



dfi 
dx 



dfi \ , . , 

Wi = -^ [Zi-Zi). 

Observe that iji can be considered as a measurement, since it is known once 
Zi is known. Also Ci is a known matrix. The quantity Ui is a noise compo- 
nent whose statistics depend on the statistics of the original noise Wi in the 
following way: 

E[u^] = 0 

TT - ixi dfj 

Ui — E[UiU^ ] — |(zi,£i_i) Wi \{zi,Xi-i) ■ 

If a Gaussian assumption is taken on the Wi then it also holds for the Ui. 
Once the approximation of equation (3.8) has been considered, the estimation 
procedure can be carried out through the KF procedure presented above. 

The use of KF for robot navigation is considered in many papers like, for 
example, [3], [4], [15], [16], [22], [28], [33], [38], [29]. 

The KF for sensor fusion is a valid framework which can handle data 
coming from many and very different sensors, as long as assumptions for its 
applicability can be satisfied and sensors statistics are known and reliable. If 
sensor statistics are unknown, different techniques must be used, as will be 
explained in the relative section. 



3.2 Sensor Fusion With Known Statistics in a Decentralised 
Architecture 

In the previous section a centralised data fusion has been considered, i.e. the 
KF algorithm is executed by a single central processor, which processes the 
data coming from all the sensors and carries out all the computation to fuse 
them. 

Now, the attention will be focused on decentralised architectures. Decen- 
tralisation is needed when the complexity of the mobile robot system requires 
computation and communication loads which can be carried out more effi- 
ciently with a parallel architecture. In addition, decentralisation and parallel 
computation improves the reliability of the overall system. It is assumed that 
the system comprises several sensors. In this case the fusion algorithm would 
require a lot of time to be executed on a single processor architecture. 
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To illustrate how a sensor fusion can be performed in a decentralised 
architecture, the implementation of the decentralised KF presented in [18] will 
be discussed. The architecture comprises several nodes, one for each sensor. 
Each sensor node is equipped with a processing unit and with communication 
facilities. It can perform autonomously its computation and communicate 
with any connected node exchanging computation results. The main features 
of this approach are: 

— the estimates obtained by each node are exactly the same estimates which 
would have been obtained using a centralised architecture; 

— the system is robust with respect to sensor failures; 

— the amount of additional computation is small compared to the computa- 
tion saved by the parallel architecture; 

— the communication overhead is also low. 

At the beginning of the algorithm, each node is given an estimate of the 
state of the system. The associated variance of this estimate is also available 
to the node. In general, data associated with the sensor of a given node have 
only a partial correspondence with the overall state of the system. In the 
main loop of the algorithm, each node takes an observation of the part of the 
state it can have access to. If the variance of these observations is known, 
the node can combine the previous estimate with the observation using the 
KF, to obtain a new partial estimate of the state. At this point, the nodes 
exchange their partial information. Each node, when data from other nodes 
are available, computes some assimilation equations to obtain a global state 
estimate. More details on this algorithm can be found in [18]. 

3.3 Sensor Fusion With Unknown Statistics 

It is possible that the uncertainty models of sensor measurements are not 
well defined. This is especially true for sensors which are signihcantly dif- 
ferent. However, it is often beneficial to use very different sensors for infor- 
mation complementarity purposes. In these cases, where uncertainty models 
are either unavailable or meaningless, the fusion is realised by some ad hoc 
techniques. In the following, some different approaches to this problem are 
illustrated. 

A first possible approach is the so-called rule-based sensor fusion. It is 
based on the definition of a set of rules which allows to handle data coming 
from the sensors. The main disadvantage of this method is that such a set of 
rules is related to a specific environment and it cannot be easily used in other 
environments. In particular, a substantial change in the rules is required if 
changes occur in the environment or in the sensor system. 

To explain this kind of approach, an example from [20] about fusion of 
sonar and infrared sensors will be described. Now, ultrasonic sensors give 
quite accurate distance measurements, even if they are characterised by wide 




262 



F. Grandoni, A. Martinelli, F. Martinelli, S. Nicosia, P. Valigi 



angles. On the other hand, infrared sensors have excellent angular resolution 
but provide poor depth measurements. Combining properly infrared and ul- 
trasonic sensors could provide good measurements in both angular and depth 
position. The following rules given in [20] allow to handle data when conflict- 
ing information is obtained: 

1. Do not consider the near- infrared sensor if the distance given by the sonar 
is greater than the maximum distance observable by the near-infrared 
sensor. 

2. If the sonar gives the maximum value it can detect, then the real distance 
is greater. 

3. If the infrared sensor goes from no-detection to detection, and the sonar 
sensor gives a distance in the range of the infrared sensor, a depth dis- 
continuity has been found. 

A typical application of rule-based sensor fusion is in map-assisted po- 
sitioning. Other fusion approaches have been developed in the case mea- 
surements statistics are unknown. Some are an extension of the KF method 
[22]; others generalise the Bayesian approaches, in particular toward Shafer- 
Dempster reasoning [8], [21], [23]. Related to this problem are also [5], [34], 
[37], [26]. 

A combination of sensor fusion with known statistics and rule-based sensor 
fusion for navigation purposes is presented in [51]. 



4 Localisation 

4.1 The Localisation Problem 

One of the key issue in autonomous robot design is localisation. A very crude 
solution to such a localisation problem is dead-reckoning, that is, integration 
of the velocity information over time. The method suffers from severe stability 
limitations, e.g. due to integration error and wheel slippage, and therefore 
cannot be used alone for medium/long distance. The usual approach is to 
integrate this simple technique with other approaches, to achieve a better 
overall accuracy. The sensor fusion techniques described in a previous section 
are frequently used to this purpose. 

To introduce a formal definition of the localisation problem, a unicycle- 
like reference robot is introduced, moving on a planar environment. It is 
possible to define the robot configuration (often referred to also as the robot 
pose) with respect to a global reference frame R by the vector Xr = [x, y, 9]'^, 
containing its position and orientation. Without considering odometry errors 
the motion equations are given by the well known unicycle equations: 



X 


= v{t) cos{9) 


(4.1) 


y 


= u{t) sin(0) 


(4.2) 


9 


= 


(4.3) 
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where iy{t) and co{t) are the translational and rotational velocity of the robot. 
The above continuous time model can be approximated via a discrete time 
one, if the period T between two discrete time instants is small enough, 
and both u{t) and oj{t) can be considered constant during such an interval, 
i.e. v{t) = v{kT) = v{k), t e [fcT, (fc + 1)T) and ui{t) = u){kT) = u;(fc), 
t G [fcT, (fc + 1)T). Then, by integration of the previous unicycle equations it 
follows: 



x{k + 1) 


= x{k) +Ti/{k)cos 


(o{k) + 


u>{k)T\ 

2 ) 


(4.4) 


y{k + l) 


= y{k) +Tiy{k) sin ^ 


^e{k) + 


u){k)T'^ 


(4.5) 


e{k + l) 


= 9{k)+u{k)T. 






(4.6) 



The dead-reckoning approach is just the use of the previous equations, 
based on knowledge of translational and rotational velocities. Actually, the 
approach is affected by several types of error, such as integration errors, wheel 
slippage, bias and wheel irregularity, just to cite a few. This can be described 
by the following discrete time model: 



x{k + 1) 


= x{k) +Ti/{k)cos 


{9{k) + 


uj{k)T'^ 
2 y 


1 -\- '^x{k 


(4.7) 


y{k + l) 


= y{k) -b Tv(k) sin | 


^9{k) + 


w(fc)T^ 


1 + Vy{k + 1) 


(4.8) 


e{k + i) 


= 9{k) + ui{k)T + ve{k + 1) 






(4.9) 



where the sequence v{k) = [Vx{k) Vy{k) V0{k)]'^ is a random sequence 
(noise) modelling the errors. Vector v{k), in the general case, has no special 
statistical properties. This leads easily to the conclusion that the above ap- 
proach can be used only over a short distance path, since over long distance 
paths errors accumulate over time and quickly lead to divergence of the robot 
position estimation error. 

Autonomous robots are usually equipped with on board sensing capabili- 
ties, allowing them to take heterogeneous measurements relating robot pose 
and the environment. Let y{k) denote the generic vector of measurement 
taken at time k. 

A common approach to the localisation problem is the use of a suitable 
estimation algorithm A, based on such a set of measurements y{k): 

Xr{k -b 1) = A{xr{k), y{k)), (4-10) 

which provides the pose estimate minimising a suitable function of the esti- 
mation error Xr{k), Xr{k) := Xr{k) — Xr{k). 

A localisation algorithm can be seen as a procedure to align a local map, 
that is the environment as perceived by the mobile robot with its own sensors, 
with a global map, that is an independent, usually a priori, description of the 
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environment, available to the robot [45]. Then, several methods can be used 
to determine the optimal alignment between the two maps, hence the optimal 
estimation of the robot pose. As an example, in the case of localisation based 
on EKF-like sensor fusion, the measurement vector y{k) is a description of the 
environment as perceived by the robot (i.e. the local map information), while 
the use of the (dynamic) model underlying the estimation phase corresponds 
to the use of the global map (or, an a priori knowledge of the environment) 
and a good alignment between the two maps is given by the EKF. 

Of course, different approaches can be cast in such a framework. For 
example, in [45] a method is proposed, which makes use of the local and 
global map concepts. The maximum-likelihood similarity measure is used, in 
order to compare the two maps, and determine the optimal pose estimation. 
The approach used to identify the best position is a hierarchical divide-and- 
conquer algorithm, spanning the pose space. The approach turns out to be 
relevant in outdoor applications, and has been motivated by long-range rover 
projects for space missions. 

To conclude the section, a quick mention of different variants of the lo- 
calisation problem is given. The problem has been studied both for indoor 
and outdoor environments. At the same time, approaches have been proposed 
that make use of beacons or artihcial landmarks, properly placed in the en- 
vironment, and approaches only based on natural features. Interaction with 
the environment, i.e. sensing of the local map, can be carried out by means 
of sonars, laser finders, camera (monocular, stereo and omnidirectional), as 
well as inertial navigation systems, the GPS and similar equipment. 

Here, a description of relevant schemes mostly for indoor environment, 
and based on natural features is given. Additional schemes can be found 
in [49], [45], [19], [42], [2] and the references therein. Other relevant schemes 
that will be briefly described in the following are based on the use of artificial 
landmarks and of nonlinear observers. 

Finally, although attention here is mainly given to single robot localisa- 
tion, the issue is under investigation also for the case of multi-robot systems, 
e.g. in case of collaborative localisation. 

4.2 The Extended Kalman Filter 

One of the most popular approaches to pose estimation, i.e. robot localisation, 
relays on the use of (Extended) Kalman filtering. The use of Kalman filtering 
for sensor fusion, henceforth for localisation, assumes the availability of a 
map of the environment. In the following such a map, which plays the role of 
the global map mentioned earlier, will be also referred to as the localisation 
map [39] . It is used during the localisation process to compute the predicted 
sensors values. 

In the following it will be considered the case (widely treated in literature) 
of sonar sensors. Given a robot conhguration Xr, the sonar expected value in 
Xr must be known. Consider an indoor bi-dimensional environment described 
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through straight segments representing the main walls. It is possible to use as 
prediction function for sonar Si, modelled as a point Sy^i), the minimum 
Euclidean distance between Si and a point, X = {xi,yi), belonging to a wall, 
so that the segment connecting Si with X is contained in the sonar beam- 
width. The predicted reading for the z*^ sonar is then given by: 

hi(^k,X‘r') — (4.11) 

In the implementation of the EKF a threshold is often introduced: it 
depends on the distance covered by the robot during one cycle time and on 
the sonar noise; when the distance between a sonar data and the relative 
sonar prediction exceeds the threshold, the sonar data is considered incorrect 
and ignored in the localisation process. On the other hand all the sonar data 
can be used to update a map describing obstacles positions. 

Denote by y{k) the vector containing sonar data at time k. These data 
depend both on the robot configuration and on the environment, according 
to (4.11): 

y{k) = h{k,Xr) + w{k) (4-12) 

where w{k) (the sonar error) is assumed a zero-mean, white, Gaussian mea- 
surement noise with covariance matrix R(k). 

Let Xr{k\k) := [x{k\k) y{k\k) 9{k\k)]^ denote the conditioned state 
configuration of the robot, i.e. the estimate at time k based on the first k 
vectors of sonar data. 

The solution of the localisation problem based on the EKF consists in 
the design of an algorithm A providing a sequence of configuration estimates 
Xr{k + l\k + 1) of the robot. 



Xr{k + l\k + 1) = A{xr{k\k),y{k + 1)). (4-13) 

One cycle of the estimation algorithm will therefore consist in mapping the 
estimate Xr{k\k) and the associated state error covariance matrix P{k\k) = 
E{xr{k\k) Xr{k\k)'^} into the corresponding variables at the next stage Xr{k+ 
l\k + 1) and P{k + l|fc -I- 1). 

The EKF approach to this problem is briefly described in the following. 
Basically, it consists in the iteration of the two phases of pose prediction and 
pose estimation. 

4.2.1 Prediction Phase. Starting from the configuration Xr{k\k) esti- 
mated at time k using sonar data, the configuration at the time fc -I- 1, i.e. 
Xr{k + l\k), is predicted. 



x(k + l\k) = x{k\k) -f Ti/{k) cos ( 9{k\k) + j 

mT \ 



(4.14) 



= y(k\k) +Tv{k)s\w[9{k\k) 
= 9{k\k)+uj{k)T. 



y{k + l\k + 1) 
9{k + l\k + 1) 



2 



(4.15) 

(4.16) 
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The associated state error covariance matrix is 

P{k + l|fc) = F{k)P{k\k)F{kf + Q(fc, 1) (4.17) 

where F{k) is the Jacobian of the motion equations (4.4)-(4.6) (written in 
compact matrix form as Xr{k + 1) = f{k,Xr{k)) + u{k + 1)) with respect to 
the robot configuration, that is: 

F{k)-.= [Vxf{k,X)]^^^^^^^^^. (4.18) 

From the localisation map and the previous predicted configuration av(fc+ 
l|fc) of the robot, relation (4.11) allows to predict the sonar measurement at 
time (fc + 1) according to: 

y{k + 1) = h{k,Xr{k + l\k)). (4.19) 

4.2.2 Estimation Phase. The estimation phase is based on the computa- 
tion of the estimation error, defined as the difference between the estimated 
measurements and the actual sonar readings: 

y(k + l) :=y{k + l)-y{k + l), (4.20) 

whose covariance is 

S{k + 1) = H{k + 1)P{K -T l\k)H{k + if + R{k + 1) (4.21) 

where Fl{k) is the Jacobian of the function /i(fc, X) with respect to the robot 
configuration, i.e. 

H{k + 1) = + 1. . (4.22) 

The final step consists in the computation of the Kalman gain K{k + 1) 
in order to compute Xr{k + l\k 1) and its covariance P(fc-|-l|fc + l). In our 
notation: 

K{k + 1) = P{k + l\k)H{k + lf S~^{k + l) (4.23) 

Xr{k + l\k + l) = Xr{k + l\k) F K{k F l)y{k + 1) (4.24) 

P{kFl\kFl) = [I - K{kFl)H{kFlf]P{kFl\k). (4.25) 

4.3 Adaptive Extended Kalman Filter 

One of the problems arising in EKF use for sensor fusion is the selection of 
proper values for the input and measurement noise covariance matrices. Of- 
ten, the magnitude of these matrices has an impact on the performance of the 
overall localisation scheme. Such a problem is particularly relevant whenever 
the robot moves in complex or quickly changing environments. A possible 
approach to overcome this problem is the use of adaptation mechanisms to 
estimate the covariance matrices. The Adaptive EKF proposed in [25] is along 
this line. Its design takes into account filter divergence and complexity of the 
implementation. 

A different approach to the same problem is proposed in [49]; it is based 
on joint Bayesan estimation and Kalman filtering. 
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4.4 Integrating Vision and Sonar 

The solution to the localisation problem is often based on the use of vision 
systems too. In addition to the approach known as visual feedback [40], [54], 
and which is outside the scope of this text, vision data can be integrated 
with other sensors, along the lines briefly outlined in the first section of this 
chapter. In this case, the use of artificial landmarks is common. 

Among many others, the approach presented in [44] , [6] uses an integration 
scheme suitable for open space movements, comprising a vision system to 
correct the robot pose in the proximity of critical points, such as in the 
proximity of doors that have to be crossed. Those critical points are labeled 
by means of landmarks. The use of beacons or artificial landmarks is quite 
common in structured environment [32], [41], [24], such as offices, academic 
laboratories, and appear to be reasonable also in other scenarios, such as 
hospitals or museums. One of the main advantages of artificial landmarks 
is that they can be used to encode in a unique manner (e.g. by using bar 
code landmark) specific location within the map, so that upon detection of 
a single landmark, the robot can very precisely determine its position and 
orientation. 

One of the main problems raised by vision systems is computational com- 
plexity. 

4.5 Observability and Localisation 

The interpretation of the localisation problem as an observability one is in- 
teresting and yields nice solutions. In [7], [14] the problem is formulated in 
terms of the observability of a dynamical system, comprising both the mobile 
robot and a set of landmarks, together with the nonlinear output equations 
relating robot configuration and landmark positions to the actual measure- 
ments. The overall system turns out to be nonlinear, and suitable tools for 
nonlinear system analysis are used to study observability, and to design non- 
linear observers solving the localisation problem. One of the key problems 
that has been considered, and for which a possible solution is given, is that 
of singularity- avoidance. 

4.6 Simultaneous Localisation and Map Construction 

In a localisation problem, it is possible to efficiently exploit sensor readings 
only if an updated map of the environment is available. Some approaches have 
been proposed, to carry out simultaneous localisation and map construction. 

In [2], the problem is formulated as a joint estimation of the robot pose 
and the environment parameters. The environment map is modelled by a 
collection of straight line segments, each segment being characterised by a set 
of four parameters. Hence, the solution is an algorithm providing sequences 
of estimates of pose and map data. Sensory data are given by odometry 
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and sonar readings. To cope with the complexity of the overall problem, a 
decomposition approach has been chosen, whereby a localisation phase and 
a map updating phase are executed iteratively. Experimental activities show 
that the approach has adequate performance. 

The same problem has been studied also in [13], where a laser rangefinder 
and a CCD camera are used to drive an EKF updating estimate of a state 
vector comprising both robot pose and environment characterisation. 

4.7 Multi-robot Localisation 

The localisation problem described above, and the corresponding solutions, 
are well suited for the case of a single robot. A more complex localisation 
problem can be formulated if a team of mobile robots has to be considered. 
In this case, a simple approach would be that of running, on each single robot, 
one of the algorithms described earlier, depending on the available sensory 
equipment. 

Nevertheless, some form of cooperation or collaboration among the robots 
may lead to a better overall accuracy. In the general case, it may occur that 
the robots have an heterogeneous sensory system, some robots being equipped 
with simple and low cost systems, and some other ones being fitted out with 
more powerful and accurate equipment. This multi-robot localisation is a 
recent issue. At the best of the author knowledge, initial contributions on the 
subject are [31], [30], based on the concept of “portable landmarks”. Other 
interesting contributions have been given in [48], [47], and [11], although 
the latter one is only partially centered on localisation. The approach in 
[48], called by the authors collective localisation, is worth mentioning here, 
since it is based on the use of a centralised scheme, implementing a single 
KF, estimating the pose of each robot in the team. The scheme fuses the 
information provided by the sensors (possible heterogeneous) on board of the 
individual robots, taking into account relationship among the collected data. 
Then, based on this initial idea, a distributed algorithm is proposed, so that 
eventually properly modified Kalman filters are implemented on each robot. 
Experimental results are presented, showing the benefit of the approach. 

4.8 Some Experimental Results 

The experiments described in the following [39] , have been carried out on the 
Nomad 150 platform (able to move with a maximum translational velocity 
of about 0.3 ms“^) available at the Robotics and Automation Laboratory, 
Department of Computer Science, Systems and Production, University of 
Rome Tor Vergata. Related results have been also presented in [43], [50]. 

4.8.1 Sonar Sensors. The sonar ranging system consists of 16 independent 
channels. This system can give range information from 0.4 m to 6.5 m with 
1% accuracy over the entire range. It measures the time of flight ranging 
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sensor based upon the return time of an acoustic signal. Each sonar sensor 
has a beamwidth of approximately 22.5. 

4.8.2 Results. The experiments were carried out moving the NomadlSO 
in the hallways of the Department at the University of Rome Tor Vergata 
(see Figure 4.1). From this figure it is possible to see that the path from 
the initial to the final position is chosen in order to minimise the covered 
distance. However, it is not difficult to modify the proposed algorithm in 
order to minimise a different cost function. 




Figure 4.1. The map of the environment with the path of the robot. The path is 
chosen in this case in order to minimise the distance covered by the robot 



The total cycle time for localisation (the most of that was due to radio- 
modem communication of sonars data) ranged from 0.7 s to 0.8 s. 

In Figure 4.2 odometry and the estimated position against the actual 
position are compared. One may conclude that the proposed algorithm is 
able to correctly localise the robot, hence correcting the integration error of 
a pure dead-reckoning scheme. 

The accuracy of the localisation process strongly depends on the transla- 
tional velocity, since this process occurs while the robot is moving. The data 
shown in Figure 4.2 refer to the case where the localisation is performed when 
the robot is moving with the maximum translational velocity (0.3 ms“^). It 
has been found that the error related to a motion with a speed of 0.1 ms“^ 
is about an half of the error showed in the figure. 



5 Conclusion and Future Directions 

In this chapter the use of a multi-sensor system for robot localisation has been 
considered. Robot localisation can be accomplished more efficiently using 
data coming from different sensors rather than data given by a unique sensor. 
The chapter focused on the techniques used to combine data from sensors 
with different characteristics. 
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Figure 4.2. The error in the position of the robot during navigation estimated 
with odometry data alone (dotted line) and by the EKF localisation algorithm 
implemented (solid line). The translational velocity was 0.3 ms~^ 



According to the particular environment considered (known or unknown) , 
to the kind of sensors (with known or unknown statistics) and to the compu- 
tational architecture (centralised or decentralised), different approaches can 
be used, which have been briefly considered in this chapter. A rich literature 
is related to this area of research. However, a vast array of open problems 
arises in this context. Some indications are provided in the following. 

A standard approach in localisation problems is to compare data coming 
from sensors with their predicted values, determined using an environment 
map. However, it is not uncommon in real applications that the environ- 
ment is not known or is not completely known. In this case a map must be 
constructed together with the localisation process. Some references on this 
subject are [2], [13]. 

A typical situation where a map of the environment is usually not available 
is in space exploration missions. This is an emergent area of research where 
new problems arise, also because some commonly used sensors like sonar can 
not be used. A related issue is the localisation of multi-robot teams. Different 
approaches can be considered in this case, as discussed in the localisation 
section. 
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